1 package com.irurueta.navigation.inertial.calibration.accelerometer;
2
3 import com.irurueta.algebra.Matrix;
4 import com.irurueta.algebra.WrongSizeException;
5 import com.irurueta.navigation.LockedException;
6 import com.irurueta.navigation.NotReadyException;
7 import com.irurueta.navigation.frames.CoordinateTransformation;
8 import com.irurueta.navigation.frames.ECEFFrame;
9 import com.irurueta.navigation.frames.FrameType;
10 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11 import com.irurueta.navigation.frames.NEDFrame;
12 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14 import com.irurueta.navigation.inertial.BodyKinematics;
15 import com.irurueta.navigation.inertial.ECEFGravity;
16 import com.irurueta.navigation.inertial.ECEFPosition;
17 import com.irurueta.navigation.inertial.ECEFVelocity;
18 import com.irurueta.navigation.inertial.NEDPosition;
19 import com.irurueta.navigation.inertial.NEDVelocity;
20 import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
21 import com.irurueta.navigation.inertial.calibration.CalibrationException;
22 import com.irurueta.navigation.inertial.calibration.IMUErrors;
23 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
24 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
25 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
26 import com.irurueta.statistics.UniformRandomizer;
27 import com.irurueta.units.Acceleration;
28 import com.irurueta.units.AccelerationUnit;
29 import org.junit.Test;
30
31 import java.util.ArrayList;
32 import java.util.Collection;
33 import java.util.Collections;
34 import java.util.List;
35 import java.util.Random;
36
37 import static org.junit.Assert.*;
38
39 public class KnownGravityNormAccelerometerCalibratorTest implements
40 KnownGravityNormAccelerometerCalibratorListener {
41
42 private static final double TIME_INTERVAL_SECONDS = 0.02;
43
44 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
45 private static final double DEG_TO_RAD = 0.01745329252;
46
47 private static final double MIN_ANGLE_DEGREES = -180.0;
48 private static final double MAX_ANGLE_DEGREES = 180.0;
49
50 private static final double MIN_LATITUDE_DEGREES = -90.0;
51 private static final double MAX_LATITUDE_DEGREES = 90.0;
52 private static final double MIN_LONGITUDE_DEGREES = -180.0;
53 private static final double MAX_LONGITUDE_DEGREES = 180.0;
54 private static final double MIN_HEIGHT = -50.0;
55 private static final double MAX_HEIGHT = 50.0;
56
57 private static final int LARGE_MEASUREMENT_NUMBER = 100000;
58
59 private static final double ABSOLUTE_ERROR = 1e-8;
60 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
61 private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
62
63 private static final int TIMES = 100;
64
65 private int mCalibrateStart;
66 private int mCalibrateEnd;
67
68 @Test
69 public void testConstructor1() throws WrongSizeException {
70 final KnownGravityNormAccelerometerCalibrator calibrator =
71 new KnownGravityNormAccelerometerCalibrator();
72
73
74 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
75 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
76 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
77 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
78 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
79 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
80 final Acceleration bx2 = new Acceleration(0.0,
81 AccelerationUnit.FEET_PER_SQUARED_SECOND);
82 calibrator.getInitialBiasXAsAcceleration(bx2);
83 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
84 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
85 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
86 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
87 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
88 final Acceleration by2 = new Acceleration(0.0,
89 AccelerationUnit.FEET_PER_SQUARED_SECOND);
90 calibrator.getInitialBiasYAsAcceleration(by2);
91 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
92 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
93 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
94 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
95 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
96 final Acceleration bz2 = new Acceleration(0.0,
97 AccelerationUnit.FEET_PER_SQUARED_SECOND);
98 calibrator.getInitialBiasZAsAcceleration(bz2);
99 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
100 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
101 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
102 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
103 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
104 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
105 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
106 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
107 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
108 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
109 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
110 final double[] bias1 = calibrator.getInitialBias();
111 assertArrayEquals(bias1, new double[3], 0.0);
112 final double[] bias2 = new double[3];
113 calibrator.getInitialBias(bias2);
114 assertArrayEquals(bias1, bias2, 0.0);
115 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
116 assertEquals(b1, new Matrix(3, 1));
117 final Matrix b2 = new Matrix(3, 1);
118 calibrator.getInitialBiasAsMatrix(b2);
119 assertEquals(b1, b2);
120 final Matrix ma1 = calibrator.getInitialMa();
121 assertEquals(ma1, new Matrix(3, 3));
122 final Matrix ma2 = new Matrix(3, 3);
123 calibrator.getInitialMa(ma2);
124 assertEquals(ma1, ma2);
125 assertNull(calibrator.getMeasurements());
126 assertFalse(calibrator.isCommonAxisUsed());
127 assertNull(calibrator.getListener());
128 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
129 assertFalse(calibrator.isReady());
130 assertFalse(calibrator.isRunning());
131 assertNull(calibrator.getEstimatedBiases());
132 assertFalse(calibrator.getEstimatedBiases(null));
133 assertNull(calibrator.getEstimatedBiasesAsMatrix());
134 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
135 assertNull(calibrator.getEstimatedBiasFx());
136 assertNull(calibrator.getEstimatedBiasFy());
137 assertNull(calibrator.getEstimatedBiasFz());
138 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
139 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
140 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
141 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
142 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
143 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
144 assertNull(calibrator.getEstimatedMa());
145 assertNull(calibrator.getEstimatedSx());
146 assertNull(calibrator.getEstimatedSy());
147 assertNull(calibrator.getEstimatedSz());
148 assertNull(calibrator.getEstimatedMxy());
149 assertNull(calibrator.getEstimatedMxz());
150 assertNull(calibrator.getEstimatedMyx());
151 assertNull(calibrator.getEstimatedMyz());
152 assertNull(calibrator.getEstimatedMzx());
153 assertNull(calibrator.getEstimatedMzy());
154 assertNull(calibrator.getEstimatedCovariance());
155 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
156 assertNull(calibrator.getGroundTruthGravityNorm());
157 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
158 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
159 }
160
161 @Test
162 public void testConstructor2() throws WrongSizeException {
163 final KnownGravityNormAccelerometerCalibrator calibrator =
164 new KnownGravityNormAccelerometerCalibrator(this);
165
166
167 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
168 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
169 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
170 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
171 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
172 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
173 final Acceleration bx2 = new Acceleration(0.0,
174 AccelerationUnit.FEET_PER_SQUARED_SECOND);
175 calibrator.getInitialBiasXAsAcceleration(bx2);
176 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
177 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
178 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
179 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
180 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
181 final Acceleration by2 = new Acceleration(0.0,
182 AccelerationUnit.FEET_PER_SQUARED_SECOND);
183 calibrator.getInitialBiasYAsAcceleration(by2);
184 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
185 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
186 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
187 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
188 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
189 final Acceleration bz2 = new Acceleration(0.0,
190 AccelerationUnit.FEET_PER_SQUARED_SECOND);
191 calibrator.getInitialBiasZAsAcceleration(bz2);
192 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
193 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
194 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
195 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
196 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
197 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
198 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
199 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
200 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
201 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
202 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
203 final double[] bias1 = calibrator.getInitialBias();
204 assertArrayEquals(bias1, new double[3], 0.0);
205 final double[] bias2 = new double[3];
206 calibrator.getInitialBias(bias2);
207 assertArrayEquals(bias1, bias2, 0.0);
208 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
209 assertEquals(b1, new Matrix(3, 1));
210 final Matrix b2 = new Matrix(3, 1);
211 calibrator.getInitialBiasAsMatrix(b2);
212 assertEquals(b1, b2);
213 final Matrix ma1 = calibrator.getInitialMa();
214 assertEquals(ma1, new Matrix(3, 3));
215 final Matrix ma2 = new Matrix(3, 3);
216 calibrator.getInitialMa(ma2);
217 assertEquals(ma1, ma2);
218 assertNull(calibrator.getMeasurements());
219 assertFalse(calibrator.isCommonAxisUsed());
220 assertSame(calibrator.getListener(), this);
221 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
222 assertFalse(calibrator.isReady());
223 assertFalse(calibrator.isRunning());
224 assertNull(calibrator.getEstimatedBiases());
225 assertFalse(calibrator.getEstimatedBiases(null));
226 assertNull(calibrator.getEstimatedBiasesAsMatrix());
227 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
228 assertNull(calibrator.getEstimatedBiasFx());
229 assertNull(calibrator.getEstimatedBiasFy());
230 assertNull(calibrator.getEstimatedBiasFz());
231 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
232 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
233 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
234 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
235 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
236 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
237 assertNull(calibrator.getEstimatedMa());
238 assertNull(calibrator.getEstimatedSx());
239 assertNull(calibrator.getEstimatedSy());
240 assertNull(calibrator.getEstimatedSz());
241 assertNull(calibrator.getEstimatedMxy());
242 assertNull(calibrator.getEstimatedMxz());
243 assertNull(calibrator.getEstimatedMyx());
244 assertNull(calibrator.getEstimatedMyz());
245 assertNull(calibrator.getEstimatedMzx());
246 assertNull(calibrator.getEstimatedMzy());
247 assertNull(calibrator.getEstimatedCovariance());
248 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
249 assertNull(calibrator.getGroundTruthGravityNorm());
250 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
251 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
252 }
253
254 @Test
255 public void testConstructor3() throws WrongSizeException {
256 final Collection<StandardDeviationBodyKinematics> measurements =
257 Collections.emptyList();
258 final KnownGravityNormAccelerometerCalibrator calibrator =
259 new KnownGravityNormAccelerometerCalibrator(measurements);
260
261
262 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
263 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
264 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
265 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
266 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
267 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
268 final Acceleration bx2 = new Acceleration(0.0,
269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
270 calibrator.getInitialBiasXAsAcceleration(bx2);
271 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
272 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
273 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
274 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
275 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
276 final Acceleration by2 = new Acceleration(0.0,
277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
278 calibrator.getInitialBiasYAsAcceleration(by2);
279 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
280 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
281 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
282 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
283 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
284 final Acceleration bz2 = new Acceleration(0.0,
285 AccelerationUnit.FEET_PER_SQUARED_SECOND);
286 calibrator.getInitialBiasZAsAcceleration(bz2);
287 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
288 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
289 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
290 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
291 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
292 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
293 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
294 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
295 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
296 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
297 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
298 final double[] bias1 = calibrator.getInitialBias();
299 assertArrayEquals(bias1, new double[3], 0.0);
300 final double[] bias2 = new double[3];
301 calibrator.getInitialBias(bias2);
302 assertArrayEquals(bias1, bias2, 0.0);
303 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
304 assertEquals(b1, new Matrix(3, 1));
305 final Matrix b2 = new Matrix(3, 1);
306 calibrator.getInitialBiasAsMatrix(b2);
307 assertEquals(b1, b2);
308 final Matrix ma1 = calibrator.getInitialMa();
309 assertEquals(ma1, new Matrix(3, 3));
310 final Matrix ma2 = new Matrix(3, 3);
311 calibrator.getInitialMa(ma2);
312 assertEquals(ma1, ma2);
313 assertSame(calibrator.getMeasurements(), measurements);
314 assertFalse(calibrator.isCommonAxisUsed());
315 assertNull(calibrator.getListener());
316 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
317 assertFalse(calibrator.isReady());
318 assertFalse(calibrator.isRunning());
319 assertNull(calibrator.getEstimatedBiases());
320 assertFalse(calibrator.getEstimatedBiases(null));
321 assertNull(calibrator.getEstimatedBiasesAsMatrix());
322 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
323 assertNull(calibrator.getEstimatedBiasFx());
324 assertNull(calibrator.getEstimatedBiasFy());
325 assertNull(calibrator.getEstimatedBiasFz());
326 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
327 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
328 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
329 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
330 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
331 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
332 assertNull(calibrator.getEstimatedMa());
333 assertNull(calibrator.getEstimatedSx());
334 assertNull(calibrator.getEstimatedSy());
335 assertNull(calibrator.getEstimatedSz());
336 assertNull(calibrator.getEstimatedMxy());
337 assertNull(calibrator.getEstimatedMxz());
338 assertNull(calibrator.getEstimatedMyx());
339 assertNull(calibrator.getEstimatedMyz());
340 assertNull(calibrator.getEstimatedMzx());
341 assertNull(calibrator.getEstimatedMzy());
342 assertNull(calibrator.getEstimatedCovariance());
343 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
344 assertNull(calibrator.getGroundTruthGravityNorm());
345 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
346 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
347 }
348
349 @Test
350 public void testConstructor4() throws WrongSizeException {
351 final Collection<StandardDeviationBodyKinematics> measurements =
352 Collections.emptyList();
353 final KnownGravityNormAccelerometerCalibrator calibrator =
354 new KnownGravityNormAccelerometerCalibrator(measurements,
355 this);
356
357
358 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
359 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
360 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
361 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
362 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
363 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
364 final Acceleration bx2 = new Acceleration(0.0,
365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
366 calibrator.getInitialBiasXAsAcceleration(bx2);
367 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
368 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
369 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
370 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
371 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
372 final Acceleration by2 = new Acceleration(0.0,
373 AccelerationUnit.FEET_PER_SQUARED_SECOND);
374 calibrator.getInitialBiasYAsAcceleration(by2);
375 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
376 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
377 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
378 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
379 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
380 final Acceleration bz2 = new Acceleration(0.0,
381 AccelerationUnit.FEET_PER_SQUARED_SECOND);
382 calibrator.getInitialBiasZAsAcceleration(bz2);
383 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
384 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
385 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
386 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
387 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
388 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
389 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
390 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
391 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
392 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
393 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
394 final double[] bias1 = calibrator.getInitialBias();
395 assertArrayEquals(bias1, new double[3], 0.0);
396 final double[] bias2 = new double[3];
397 calibrator.getInitialBias(bias2);
398 assertArrayEquals(bias1, bias2, 0.0);
399 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
400 assertEquals(b1, new Matrix(3, 1));
401 final Matrix b2 = new Matrix(3, 1);
402 calibrator.getInitialBiasAsMatrix(b2);
403 assertEquals(b1, b2);
404 final Matrix ma1 = calibrator.getInitialMa();
405 assertEquals(ma1, new Matrix(3, 3));
406 final Matrix ma2 = new Matrix(3, 3);
407 calibrator.getInitialMa(ma2);
408 assertEquals(ma1, ma2);
409 assertSame(calibrator.getMeasurements(), measurements);
410 assertFalse(calibrator.isCommonAxisUsed());
411 assertSame(calibrator.getListener(), this);
412 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
413 assertFalse(calibrator.isReady());
414 assertFalse(calibrator.isRunning());
415 assertNull(calibrator.getEstimatedBiases());
416 assertFalse(calibrator.getEstimatedBiases(null));
417 assertNull(calibrator.getEstimatedBiasesAsMatrix());
418 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
419 assertNull(calibrator.getEstimatedBiasFx());
420 assertNull(calibrator.getEstimatedBiasFy());
421 assertNull(calibrator.getEstimatedBiasFz());
422 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
423 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
424 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
425 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
426 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
427 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
428 assertNull(calibrator.getEstimatedMa());
429 assertNull(calibrator.getEstimatedSx());
430 assertNull(calibrator.getEstimatedSy());
431 assertNull(calibrator.getEstimatedSz());
432 assertNull(calibrator.getEstimatedMxy());
433 assertNull(calibrator.getEstimatedMxz());
434 assertNull(calibrator.getEstimatedMyx());
435 assertNull(calibrator.getEstimatedMyz());
436 assertNull(calibrator.getEstimatedMzx());
437 assertNull(calibrator.getEstimatedMzy());
438 assertNull(calibrator.getEstimatedCovariance());
439 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
440 assertNull(calibrator.getGroundTruthGravityNorm());
441 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
442 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
443 }
444
445 @Test
446 public void testConstructor5() throws WrongSizeException {
447 final KnownGravityNormAccelerometerCalibrator calibrator =
448 new KnownGravityNormAccelerometerCalibrator(true);
449
450
451 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
452 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
453 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
454 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
455 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
456 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
457 final Acceleration bx2 = new Acceleration(0.0,
458 AccelerationUnit.FEET_PER_SQUARED_SECOND);
459 calibrator.getInitialBiasXAsAcceleration(bx2);
460 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
461 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
462 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
463 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
464 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
465 final Acceleration by2 = new Acceleration(0.0,
466 AccelerationUnit.FEET_PER_SQUARED_SECOND);
467 calibrator.getInitialBiasYAsAcceleration(by2);
468 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
469 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
470 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
471 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
472 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
473 final Acceleration bz2 = new Acceleration(0.0,
474 AccelerationUnit.FEET_PER_SQUARED_SECOND);
475 calibrator.getInitialBiasZAsAcceleration(bz2);
476 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
477 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
478 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
479 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
480 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
481 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
482 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
483 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
484 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
485 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
486 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
487 final double[] bias1 = calibrator.getInitialBias();
488 assertArrayEquals(bias1, new double[3], 0.0);
489 final double[] bias2 = new double[3];
490 calibrator.getInitialBias(bias2);
491 assertArrayEquals(bias1, bias2, 0.0);
492 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
493 assertEquals(b1, new Matrix(3, 1));
494 final Matrix b2 = new Matrix(3, 1);
495 calibrator.getInitialBiasAsMatrix(b2);
496 assertEquals(b1, b2);
497 final Matrix ma1 = calibrator.getInitialMa();
498 assertEquals(ma1, new Matrix(3, 3));
499 final Matrix ma2 = new Matrix(3, 3);
500 calibrator.getInitialMa(ma2);
501 assertEquals(ma1, ma2);
502 assertNull(calibrator.getMeasurements());
503 assertTrue(calibrator.isCommonAxisUsed());
504 assertNull(calibrator.getListener());
505 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
506 assertFalse(calibrator.isReady());
507 assertFalse(calibrator.isRunning());
508 assertNull(calibrator.getEstimatedBiases());
509 assertFalse(calibrator.getEstimatedBiases(null));
510 assertNull(calibrator.getEstimatedBiasesAsMatrix());
511 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
512 assertNull(calibrator.getEstimatedBiasFx());
513 assertNull(calibrator.getEstimatedBiasFy());
514 assertNull(calibrator.getEstimatedBiasFz());
515 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
516 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
517 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
518 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
519 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
520 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
521 assertNull(calibrator.getEstimatedMa());
522 assertNull(calibrator.getEstimatedSx());
523 assertNull(calibrator.getEstimatedSy());
524 assertNull(calibrator.getEstimatedSz());
525 assertNull(calibrator.getEstimatedMxy());
526 assertNull(calibrator.getEstimatedMxz());
527 assertNull(calibrator.getEstimatedMyx());
528 assertNull(calibrator.getEstimatedMyz());
529 assertNull(calibrator.getEstimatedMzx());
530 assertNull(calibrator.getEstimatedMzy());
531 assertNull(calibrator.getEstimatedCovariance());
532 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
533 assertNull(calibrator.getGroundTruthGravityNorm());
534 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
535 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
536 }
537
538 @Test
539 public void testConstructor6() throws WrongSizeException {
540 final KnownGravityNormAccelerometerCalibrator calibrator =
541 new KnownGravityNormAccelerometerCalibrator(
542 true, this);
543
544
545 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
546 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
547 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
548 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
549 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
550 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
551 final Acceleration bx2 = new Acceleration(0.0,
552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
553 calibrator.getInitialBiasXAsAcceleration(bx2);
554 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
555 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
556 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
557 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
558 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
559 final Acceleration by2 = new Acceleration(0.0,
560 AccelerationUnit.FEET_PER_SQUARED_SECOND);
561 calibrator.getInitialBiasYAsAcceleration(by2);
562 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
563 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
564 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
565 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
566 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
567 final Acceleration bz2 = new Acceleration(0.0,
568 AccelerationUnit.FEET_PER_SQUARED_SECOND);
569 calibrator.getInitialBiasZAsAcceleration(bz2);
570 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
571 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
572 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
573 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
574 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
575 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
576 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
577 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
578 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
579 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
580 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
581 final double[] bias1 = calibrator.getInitialBias();
582 assertArrayEquals(bias1, new double[3], 0.0);
583 final double[] bias2 = new double[3];
584 calibrator.getInitialBias(bias2);
585 assertArrayEquals(bias1, bias2, 0.0);
586 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
587 assertEquals(b1, new Matrix(3, 1));
588 final Matrix b2 = new Matrix(3, 1);
589 calibrator.getInitialBiasAsMatrix(b2);
590 assertEquals(b1, b2);
591 final Matrix ma1 = calibrator.getInitialMa();
592 assertEquals(ma1, new Matrix(3, 3));
593 final Matrix ma2 = new Matrix(3, 3);
594 calibrator.getInitialMa(ma2);
595 assertEquals(ma1, ma2);
596 assertNull(calibrator.getMeasurements());
597 assertTrue(calibrator.isCommonAxisUsed());
598 assertSame(calibrator.getListener(), this);
599 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
600 assertFalse(calibrator.isReady());
601 assertFalse(calibrator.isRunning());
602 assertNull(calibrator.getEstimatedBiases());
603 assertFalse(calibrator.getEstimatedBiases(null));
604 assertNull(calibrator.getEstimatedBiasesAsMatrix());
605 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
606 assertNull(calibrator.getEstimatedBiasFx());
607 assertNull(calibrator.getEstimatedBiasFy());
608 assertNull(calibrator.getEstimatedBiasFz());
609 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
610 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
611 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
612 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
613 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
614 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
615 assertNull(calibrator.getEstimatedMa());
616 assertNull(calibrator.getEstimatedSx());
617 assertNull(calibrator.getEstimatedSy());
618 assertNull(calibrator.getEstimatedSz());
619 assertNull(calibrator.getEstimatedMxy());
620 assertNull(calibrator.getEstimatedMxz());
621 assertNull(calibrator.getEstimatedMyx());
622 assertNull(calibrator.getEstimatedMyz());
623 assertNull(calibrator.getEstimatedMzx());
624 assertNull(calibrator.getEstimatedMzy());
625 assertNull(calibrator.getEstimatedCovariance());
626 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
627 assertNull(calibrator.getGroundTruthGravityNorm());
628 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
629 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
630 }
631
632 @Test
633 public void testConstructor7() throws WrongSizeException {
634 final Collection<StandardDeviationBodyKinematics> measurements =
635 Collections.emptyList();
636 final KnownGravityNormAccelerometerCalibrator calibrator =
637 new KnownGravityNormAccelerometerCalibrator(measurements,
638 true);
639
640
641 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
642 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
643 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
644 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
645 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
646 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
647 final Acceleration bx2 = new Acceleration(0.0,
648 AccelerationUnit.FEET_PER_SQUARED_SECOND);
649 calibrator.getInitialBiasXAsAcceleration(bx2);
650 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
651 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
652 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
653 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
654 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
655 final Acceleration by2 = new Acceleration(0.0,
656 AccelerationUnit.FEET_PER_SQUARED_SECOND);
657 calibrator.getInitialBiasYAsAcceleration(by2);
658 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
659 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
660 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
661 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
662 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
663 final Acceleration bz2 = new Acceleration(0.0,
664 AccelerationUnit.FEET_PER_SQUARED_SECOND);
665 calibrator.getInitialBiasZAsAcceleration(bz2);
666 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
667 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
668 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
669 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
670 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
671 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
672 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
673 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
674 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
675 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
676 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
677 final double[] bias1 = calibrator.getInitialBias();
678 assertArrayEquals(bias1, new double[3], 0.0);
679 final double[] bias2 = new double[3];
680 calibrator.getInitialBias(bias2);
681 assertArrayEquals(bias1, bias2, 0.0);
682 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
683 assertEquals(b1, new Matrix(3, 1));
684 final Matrix b2 = new Matrix(3, 1);
685 calibrator.getInitialBiasAsMatrix(b2);
686 assertEquals(b1, b2);
687 final Matrix ma1 = calibrator.getInitialMa();
688 assertEquals(ma1, new Matrix(3, 3));
689 final Matrix ma2 = new Matrix(3, 3);
690 calibrator.getInitialMa(ma2);
691 assertEquals(ma1, ma2);
692 assertSame(calibrator.getMeasurements(), measurements);
693 assertTrue(calibrator.isCommonAxisUsed());
694 assertNull(calibrator.getListener());
695 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
696 assertFalse(calibrator.isReady());
697 assertFalse(calibrator.isRunning());
698 assertNull(calibrator.getEstimatedBiases());
699 assertFalse(calibrator.getEstimatedBiases(null));
700 assertNull(calibrator.getEstimatedBiasesAsMatrix());
701 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
702 assertNull(calibrator.getEstimatedBiasFx());
703 assertNull(calibrator.getEstimatedBiasFy());
704 assertNull(calibrator.getEstimatedBiasFz());
705 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
706 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
707 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
708 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
709 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
710 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
711 assertNull(calibrator.getEstimatedMa());
712 assertNull(calibrator.getEstimatedSx());
713 assertNull(calibrator.getEstimatedSy());
714 assertNull(calibrator.getEstimatedSz());
715 assertNull(calibrator.getEstimatedMxy());
716 assertNull(calibrator.getEstimatedMxz());
717 assertNull(calibrator.getEstimatedMyx());
718 assertNull(calibrator.getEstimatedMyz());
719 assertNull(calibrator.getEstimatedMzx());
720 assertNull(calibrator.getEstimatedMzy());
721 assertNull(calibrator.getEstimatedCovariance());
722 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
723 assertNull(calibrator.getGroundTruthGravityNorm());
724 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
725 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
726 }
727
728 @Test
729 public void testConstructor8() throws WrongSizeException {
730 final Collection<StandardDeviationBodyKinematics> measurements =
731 Collections.emptyList();
732 final KnownGravityNormAccelerometerCalibrator calibrator =
733 new KnownGravityNormAccelerometerCalibrator(measurements,
734 true, this);
735
736
737 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
738 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
739 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
740 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
741 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
742 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
743 final Acceleration bx2 = new Acceleration(0.0,
744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
745 calibrator.getInitialBiasXAsAcceleration(bx2);
746 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
747 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
748 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
749 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
750 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
751 final Acceleration by2 = new Acceleration(0.0,
752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
753 calibrator.getInitialBiasYAsAcceleration(by2);
754 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
755 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
756 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
757 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
758 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
759 final Acceleration bz2 = new Acceleration(0.0,
760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
761 calibrator.getInitialBiasZAsAcceleration(bz2);
762 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
763 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
765 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
766 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
767 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
768 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
769 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
770 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
771 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
772 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
773 final double[] bias1 = calibrator.getInitialBias();
774 assertArrayEquals(bias1, new double[3], 0.0);
775 final double[] bias2 = new double[3];
776 calibrator.getInitialBias(bias2);
777 assertArrayEquals(bias1, bias2, 0.0);
778 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
779 assertEquals(b1, new Matrix(3, 1));
780 final Matrix b2 = new Matrix(3, 1);
781 calibrator.getInitialBiasAsMatrix(b2);
782 assertEquals(b1, b2);
783 final Matrix ma1 = calibrator.getInitialMa();
784 assertEquals(ma1, new Matrix(3, 3));
785 final Matrix ma2 = new Matrix(3, 3);
786 calibrator.getInitialMa(ma2);
787 assertEquals(ma1, ma2);
788 assertSame(calibrator.getMeasurements(), measurements);
789 assertTrue(calibrator.isCommonAxisUsed());
790 assertSame(calibrator.getListener(), this);
791 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
792 assertFalse(calibrator.isReady());
793 assertFalse(calibrator.isRunning());
794 assertNull(calibrator.getEstimatedBiases());
795 assertFalse(calibrator.getEstimatedBiases(null));
796 assertNull(calibrator.getEstimatedBiasesAsMatrix());
797 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
798 assertNull(calibrator.getEstimatedBiasFx());
799 assertNull(calibrator.getEstimatedBiasFy());
800 assertNull(calibrator.getEstimatedBiasFz());
801 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
802 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
803 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
804 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
805 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
806 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
807 assertNull(calibrator.getEstimatedMa());
808 assertNull(calibrator.getEstimatedSx());
809 assertNull(calibrator.getEstimatedSy());
810 assertNull(calibrator.getEstimatedSz());
811 assertNull(calibrator.getEstimatedMxy());
812 assertNull(calibrator.getEstimatedMxz());
813 assertNull(calibrator.getEstimatedMyx());
814 assertNull(calibrator.getEstimatedMyz());
815 assertNull(calibrator.getEstimatedMzx());
816 assertNull(calibrator.getEstimatedMzy());
817 assertNull(calibrator.getEstimatedCovariance());
818 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
819 assertNull(calibrator.getGroundTruthGravityNorm());
820 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
821 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
822 }
823
824 @Test
825 public void testConstructor9() throws WrongSizeException {
826 final Matrix ba = generateBa();
827 final double biasX = ba.getElementAtIndex(0);
828 final double biasY = ba.getElementAtIndex(1);
829 final double biasZ = ba.getElementAtIndex(2);
830
831 final KnownGravityNormAccelerometerCalibrator calibrator =
832 new KnownGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
833
834
835 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
836 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
837 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
838 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
839 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
840 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
841 final Acceleration bx2 = new Acceleration(0.0,
842 AccelerationUnit.FEET_PER_SQUARED_SECOND);
843 calibrator.getInitialBiasXAsAcceleration(bx2);
844 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
845 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
846 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
847 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
848 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
849 final Acceleration by2 = new Acceleration(0.0,
850 AccelerationUnit.FEET_PER_SQUARED_SECOND);
851 calibrator.getInitialBiasYAsAcceleration(by2);
852 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
853 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
854 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
855 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
856 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
857 final Acceleration bz2 = new Acceleration(0.0,
858 AccelerationUnit.FEET_PER_SQUARED_SECOND);
859 calibrator.getInitialBiasZAsAcceleration(bz2);
860 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
861 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
862 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
863 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
864 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
865 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
866 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
867 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
868 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
869 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
870 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
871 final double[] bias1 = calibrator.getInitialBias();
872 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
873 final double[] bias2 = new double[3];
874 calibrator.getInitialBias(bias2);
875 assertArrayEquals(bias1, bias2, 0.0);
876 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
877 assertEquals(b1, ba);
878 final Matrix b2 = new Matrix(3, 1);
879 calibrator.getInitialBiasAsMatrix(b2);
880 assertEquals(b1, b2);
881 final Matrix ma1 = calibrator.getInitialMa();
882 assertEquals(ma1, new Matrix(3, 3));
883 final Matrix ma2 = new Matrix(3, 3);
884 calibrator.getInitialMa(ma2);
885 assertEquals(ma1, ma2);
886 assertNull(calibrator.getMeasurements());
887 assertFalse(calibrator.isCommonAxisUsed());
888 assertNull(calibrator.getListener());
889 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
890 assertFalse(calibrator.isReady());
891 assertFalse(calibrator.isRunning());
892 assertNull(calibrator.getEstimatedBiases());
893 assertFalse(calibrator.getEstimatedBiases(null));
894 assertNull(calibrator.getEstimatedBiasesAsMatrix());
895 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
896 assertNull(calibrator.getEstimatedBiasFx());
897 assertNull(calibrator.getEstimatedBiasFy());
898 assertNull(calibrator.getEstimatedBiasFz());
899 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
900 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
901 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
902 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
903 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
904 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
905 assertNull(calibrator.getEstimatedMa());
906 assertNull(calibrator.getEstimatedSx());
907 assertNull(calibrator.getEstimatedSy());
908 assertNull(calibrator.getEstimatedSz());
909 assertNull(calibrator.getEstimatedMxy());
910 assertNull(calibrator.getEstimatedMxz());
911 assertNull(calibrator.getEstimatedMyx());
912 assertNull(calibrator.getEstimatedMyz());
913 assertNull(calibrator.getEstimatedMzx());
914 assertNull(calibrator.getEstimatedMzy());
915 assertNull(calibrator.getEstimatedCovariance());
916 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
917 assertNull(calibrator.getGroundTruthGravityNorm());
918 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
919 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
920 }
921
922 @Test
923 public void testConstructor10() throws WrongSizeException {
924 final Matrix ba = generateBa();
925 final double biasX = ba.getElementAtIndex(0);
926 final double biasY = ba.getElementAtIndex(1);
927 final double biasZ = ba.getElementAtIndex(2);
928
929 final KnownGravityNormAccelerometerCalibrator calibrator =
930 new KnownGravityNormAccelerometerCalibrator(
931 biasX, biasY, biasZ, this);
932
933
934 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
935 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
936 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
937 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
938 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
939 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
940 final Acceleration bx2 = new Acceleration(0.0,
941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
942 calibrator.getInitialBiasXAsAcceleration(bx2);
943 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
944 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
945 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
946 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
947 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
948 final Acceleration by2 = new Acceleration(0.0,
949 AccelerationUnit.FEET_PER_SQUARED_SECOND);
950 calibrator.getInitialBiasYAsAcceleration(by2);
951 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
952 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
953 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
954 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
955 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
956 final Acceleration bz2 = new Acceleration(0.0,
957 AccelerationUnit.FEET_PER_SQUARED_SECOND);
958 calibrator.getInitialBiasZAsAcceleration(bz2);
959 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
960 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
961 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
962 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
963 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
964 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
965 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
966 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
967 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
968 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
969 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
970 final double[] bias1 = calibrator.getInitialBias();
971 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
972 final double[] bias2 = new double[3];
973 calibrator.getInitialBias(bias2);
974 assertArrayEquals(bias1, bias2, 0.0);
975 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
976 assertEquals(b1, ba);
977 final Matrix b2 = new Matrix(3, 1);
978 calibrator.getInitialBiasAsMatrix(b2);
979 assertEquals(b1, b2);
980 final Matrix ma1 = calibrator.getInitialMa();
981 assertEquals(ma1, new Matrix(3, 3));
982 final Matrix ma2 = new Matrix(3, 3);
983 calibrator.getInitialMa(ma2);
984 assertEquals(ma1, ma2);
985 assertNull(calibrator.getMeasurements());
986 assertFalse(calibrator.isCommonAxisUsed());
987 assertSame(calibrator.getListener(), this);
988 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
989 assertFalse(calibrator.isReady());
990 assertFalse(calibrator.isRunning());
991 assertNull(calibrator.getEstimatedBiases());
992 assertFalse(calibrator.getEstimatedBiases(null));
993 assertNull(calibrator.getEstimatedBiasesAsMatrix());
994 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
995 assertNull(calibrator.getEstimatedBiasFx());
996 assertNull(calibrator.getEstimatedBiasFy());
997 assertNull(calibrator.getEstimatedBiasFz());
998 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
999 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1000 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1001 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1002 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1003 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1004 assertNull(calibrator.getEstimatedMa());
1005 assertNull(calibrator.getEstimatedSx());
1006 assertNull(calibrator.getEstimatedSy());
1007 assertNull(calibrator.getEstimatedSz());
1008 assertNull(calibrator.getEstimatedMxy());
1009 assertNull(calibrator.getEstimatedMxz());
1010 assertNull(calibrator.getEstimatedMyx());
1011 assertNull(calibrator.getEstimatedMyz());
1012 assertNull(calibrator.getEstimatedMzx());
1013 assertNull(calibrator.getEstimatedMzy());
1014 assertNull(calibrator.getEstimatedCovariance());
1015 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1016 assertNull(calibrator.getGroundTruthGravityNorm());
1017 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1018 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1019 }
1020
1021 @Test
1022 public void testConstructor11() throws WrongSizeException {
1023 final Collection<StandardDeviationBodyKinematics> measurements =
1024 Collections.emptyList();
1025
1026 final Matrix ba = generateBa();
1027 final double biasX = ba.getElementAtIndex(0);
1028 final double biasY = ba.getElementAtIndex(1);
1029 final double biasZ = ba.getElementAtIndex(2);
1030
1031 final KnownGravityNormAccelerometerCalibrator calibrator =
1032 new KnownGravityNormAccelerometerCalibrator(measurements,
1033 biasX, biasY, biasZ);
1034
1035
1036 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1037 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1038 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1039 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1040 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1041 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1042 final Acceleration bx2 = new Acceleration(0.0,
1043 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1044 calibrator.getInitialBiasXAsAcceleration(bx2);
1045 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1046 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1047 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1048 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1049 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1050 final Acceleration by2 = new Acceleration(0.0,
1051 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1052 calibrator.getInitialBiasYAsAcceleration(by2);
1053 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1054 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1055 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1056 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1057 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1058 final Acceleration bz2 = new Acceleration(0.0,
1059 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1060 calibrator.getInitialBiasZAsAcceleration(bz2);
1061 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1062 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1063 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1064 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1065 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1066 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1067 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1068 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1069 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1070 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1071 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1072 final double[] bias1 = calibrator.getInitialBias();
1073 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1074 final double[] bias2 = new double[3];
1075 calibrator.getInitialBias(bias2);
1076 assertArrayEquals(bias1, bias2, 0.0);
1077 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1078 assertEquals(b1, ba);
1079 final Matrix b2 = new Matrix(3, 1);
1080 calibrator.getInitialBiasAsMatrix(b2);
1081 assertEquals(b1, b2);
1082 final Matrix ma1 = calibrator.getInitialMa();
1083 assertEquals(ma1, new Matrix(3, 3));
1084 final Matrix ma2 = new Matrix(3, 3);
1085 calibrator.getInitialMa(ma2);
1086 assertEquals(ma1, ma2);
1087 assertSame(calibrator.getMeasurements(), measurements);
1088 assertFalse(calibrator.isCommonAxisUsed());
1089 assertNull(calibrator.getListener());
1090 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1091 assertFalse(calibrator.isReady());
1092 assertFalse(calibrator.isRunning());
1093 assertNull(calibrator.getEstimatedBiases());
1094 assertFalse(calibrator.getEstimatedBiases(null));
1095 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1096 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1097 assertNull(calibrator.getEstimatedBiasFx());
1098 assertNull(calibrator.getEstimatedBiasFy());
1099 assertNull(calibrator.getEstimatedBiasFz());
1100 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1101 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1102 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1103 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1104 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1105 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1106 assertNull(calibrator.getEstimatedMa());
1107 assertNull(calibrator.getEstimatedSx());
1108 assertNull(calibrator.getEstimatedSy());
1109 assertNull(calibrator.getEstimatedSz());
1110 assertNull(calibrator.getEstimatedMxy());
1111 assertNull(calibrator.getEstimatedMxz());
1112 assertNull(calibrator.getEstimatedMyx());
1113 assertNull(calibrator.getEstimatedMyz());
1114 assertNull(calibrator.getEstimatedMzx());
1115 assertNull(calibrator.getEstimatedMzy());
1116 assertNull(calibrator.getEstimatedCovariance());
1117 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1118 assertNull(calibrator.getGroundTruthGravityNorm());
1119 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1120 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1121 }
1122
1123 @Test
1124 public void testConstructor12() throws WrongSizeException {
1125 final Collection<StandardDeviationBodyKinematics> measurements =
1126 Collections.emptyList();
1127
1128 final Matrix ba = generateBa();
1129 final double biasX = ba.getElementAtIndex(0);
1130 final double biasY = ba.getElementAtIndex(1);
1131 final double biasZ = ba.getElementAtIndex(2);
1132
1133 final KnownGravityNormAccelerometerCalibrator calibrator =
1134 new KnownGravityNormAccelerometerCalibrator(measurements,
1135 biasX, biasY, biasZ, this);
1136
1137
1138 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1139 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1140 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1141 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1142 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1143 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1144 final Acceleration bx2 = new Acceleration(0.0,
1145 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1146 calibrator.getInitialBiasXAsAcceleration(bx2);
1147 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1148 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1149 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1150 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1151 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1152 final Acceleration by2 = new Acceleration(0.0,
1153 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1154 calibrator.getInitialBiasYAsAcceleration(by2);
1155 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1156 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1157 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1158 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1159 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1160 final Acceleration bz2 = new Acceleration(0.0,
1161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1162 calibrator.getInitialBiasZAsAcceleration(bz2);
1163 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1164 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1165 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1166 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1167 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1168 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1169 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1170 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1171 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1172 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1173 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1174 final double[] bias1 = calibrator.getInitialBias();
1175 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1176 final double[] bias2 = new double[3];
1177 calibrator.getInitialBias(bias2);
1178 assertArrayEquals(bias1, bias2, 0.0);
1179 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1180 assertEquals(b1, ba);
1181 final Matrix b2 = new Matrix(3, 1);
1182 calibrator.getInitialBiasAsMatrix(b2);
1183 assertEquals(b1, b2);
1184 final Matrix ma1 = calibrator.getInitialMa();
1185 assertEquals(ma1, new Matrix(3, 3));
1186 final Matrix ma2 = new Matrix(3, 3);
1187 calibrator.getInitialMa(ma2);
1188 assertEquals(ma1, ma2);
1189 assertSame(calibrator.getMeasurements(), measurements);
1190 assertFalse(calibrator.isCommonAxisUsed());
1191 assertSame(calibrator.getListener(), this);
1192 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1193 assertFalse(calibrator.isReady());
1194 assertFalse(calibrator.isRunning());
1195 assertNull(calibrator.getEstimatedBiases());
1196 assertFalse(calibrator.getEstimatedBiases(null));
1197 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1198 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1199 assertNull(calibrator.getEstimatedBiasFx());
1200 assertNull(calibrator.getEstimatedBiasFy());
1201 assertNull(calibrator.getEstimatedBiasFz());
1202 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1203 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1204 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1205 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1206 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1207 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1208 assertNull(calibrator.getEstimatedMa());
1209 assertNull(calibrator.getEstimatedSx());
1210 assertNull(calibrator.getEstimatedSy());
1211 assertNull(calibrator.getEstimatedSz());
1212 assertNull(calibrator.getEstimatedMxy());
1213 assertNull(calibrator.getEstimatedMxz());
1214 assertNull(calibrator.getEstimatedMyx());
1215 assertNull(calibrator.getEstimatedMyz());
1216 assertNull(calibrator.getEstimatedMzx());
1217 assertNull(calibrator.getEstimatedMzy());
1218 assertNull(calibrator.getEstimatedCovariance());
1219 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1220 assertNull(calibrator.getGroundTruthGravityNorm());
1221 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1222 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1223 }
1224
1225 @Test
1226 public void testConstructor13() throws WrongSizeException {
1227 final Matrix ba = generateBa();
1228 final double biasX = ba.getElementAtIndex(0);
1229 final double biasY = ba.getElementAtIndex(1);
1230 final double biasZ = ba.getElementAtIndex(2);
1231
1232 final KnownGravityNormAccelerometerCalibrator calibrator =
1233 new KnownGravityNormAccelerometerCalibrator(
1234 true, biasX, biasY, biasZ);
1235
1236
1237 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1238 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1239 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1240 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1241 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1242 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1243 final Acceleration bx2 = new Acceleration(0.0,
1244 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1245 calibrator.getInitialBiasXAsAcceleration(bx2);
1246 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1247 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1248 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1249 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1250 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1251 final Acceleration by2 = new Acceleration(0.0,
1252 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1253 calibrator.getInitialBiasYAsAcceleration(by2);
1254 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1255 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1256 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1257 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1258 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1259 final Acceleration bz2 = new Acceleration(0.0,
1260 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1261 calibrator.getInitialBiasZAsAcceleration(bz2);
1262 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1263 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1264 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1265 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1266 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1267 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1268 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1269 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1270 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1271 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1272 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1273 final double[] bias1 = calibrator.getInitialBias();
1274 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1275 final double[] bias2 = new double[3];
1276 calibrator.getInitialBias(bias2);
1277 assertArrayEquals(bias1, bias2, 0.0);
1278 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1279 assertEquals(b1, ba);
1280 final Matrix b2 = new Matrix(3, 1);
1281 calibrator.getInitialBiasAsMatrix(b2);
1282 assertEquals(b1, b2);
1283 final Matrix ma1 = calibrator.getInitialMa();
1284 assertEquals(ma1, new Matrix(3, 3));
1285 final Matrix ma2 = new Matrix(3, 3);
1286 calibrator.getInitialMa(ma2);
1287 assertEquals(ma1, ma2);
1288 assertNull(calibrator.getMeasurements());
1289 assertTrue(calibrator.isCommonAxisUsed());
1290 assertNull(calibrator.getListener());
1291 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1292 assertFalse(calibrator.isReady());
1293 assertFalse(calibrator.isRunning());
1294 assertNull(calibrator.getEstimatedBiases());
1295 assertFalse(calibrator.getEstimatedBiases(null));
1296 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1297 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1298 assertNull(calibrator.getEstimatedBiasFx());
1299 assertNull(calibrator.getEstimatedBiasFy());
1300 assertNull(calibrator.getEstimatedBiasFz());
1301 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1302 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1303 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1304 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1305 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1306 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1307 assertNull(calibrator.getEstimatedMa());
1308 assertNull(calibrator.getEstimatedSx());
1309 assertNull(calibrator.getEstimatedSy());
1310 assertNull(calibrator.getEstimatedSz());
1311 assertNull(calibrator.getEstimatedMxy());
1312 assertNull(calibrator.getEstimatedMxz());
1313 assertNull(calibrator.getEstimatedMyx());
1314 assertNull(calibrator.getEstimatedMyz());
1315 assertNull(calibrator.getEstimatedMzx());
1316 assertNull(calibrator.getEstimatedMzy());
1317 assertNull(calibrator.getEstimatedCovariance());
1318 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1319 assertNull(calibrator.getGroundTruthGravityNorm());
1320 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1321 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1322 }
1323
1324 @Test
1325 public void testConstructor14() throws WrongSizeException {
1326 final Matrix ba = generateBa();
1327 final double biasX = ba.getElementAtIndex(0);
1328 final double biasY = ba.getElementAtIndex(1);
1329 final double biasZ = ba.getElementAtIndex(2);
1330
1331 final KnownGravityNormAccelerometerCalibrator calibrator =
1332 new KnownGravityNormAccelerometerCalibrator(
1333 true, biasX, biasY, biasZ,
1334 this);
1335
1336
1337 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1338 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1339 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1340 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1341 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1342 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1343 final Acceleration bx2 = new Acceleration(0.0,
1344 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1345 calibrator.getInitialBiasXAsAcceleration(bx2);
1346 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1347 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1348 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1349 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1350 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1351 final Acceleration by2 = new Acceleration(0.0,
1352 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1353 calibrator.getInitialBiasYAsAcceleration(by2);
1354 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1355 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1356 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1357 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1358 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1359 final Acceleration bz2 = new Acceleration(0.0,
1360 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1361 calibrator.getInitialBiasZAsAcceleration(bz2);
1362 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1363 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1364 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1365 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1366 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1367 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1368 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1369 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1370 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1371 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1372 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1373 final double[] bias1 = calibrator.getInitialBias();
1374 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1375 final double[] bias2 = new double[3];
1376 calibrator.getInitialBias(bias2);
1377 assertArrayEquals(bias1, bias2, 0.0);
1378 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1379 assertEquals(b1, ba);
1380 final Matrix b2 = new Matrix(3, 1);
1381 calibrator.getInitialBiasAsMatrix(b2);
1382 assertEquals(b1, b2);
1383 final Matrix ma1 = calibrator.getInitialMa();
1384 assertEquals(ma1, new Matrix(3, 3));
1385 final Matrix ma2 = new Matrix(3, 3);
1386 calibrator.getInitialMa(ma2);
1387 assertEquals(ma1, ma2);
1388 assertNull(calibrator.getMeasurements());
1389 assertTrue(calibrator.isCommonAxisUsed());
1390 assertSame(calibrator.getListener(), this);
1391 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1392 assertFalse(calibrator.isReady());
1393 assertFalse(calibrator.isRunning());
1394 assertNull(calibrator.getEstimatedBiases());
1395 assertFalse(calibrator.getEstimatedBiases(null));
1396 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1397 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1398 assertNull(calibrator.getEstimatedBiasFx());
1399 assertNull(calibrator.getEstimatedBiasFy());
1400 assertNull(calibrator.getEstimatedBiasFz());
1401 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1402 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1403 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1404 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1405 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1406 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1407 assertNull(calibrator.getEstimatedMa());
1408 assertNull(calibrator.getEstimatedSx());
1409 assertNull(calibrator.getEstimatedSy());
1410 assertNull(calibrator.getEstimatedSz());
1411 assertNull(calibrator.getEstimatedMxy());
1412 assertNull(calibrator.getEstimatedMxz());
1413 assertNull(calibrator.getEstimatedMyx());
1414 assertNull(calibrator.getEstimatedMyz());
1415 assertNull(calibrator.getEstimatedMzx());
1416 assertNull(calibrator.getEstimatedMzy());
1417 assertNull(calibrator.getEstimatedCovariance());
1418 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1419 assertNull(calibrator.getGroundTruthGravityNorm());
1420 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1421 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1422 }
1423
1424 @Test
1425 public void testConstructor15() throws WrongSizeException {
1426 final Collection<StandardDeviationBodyKinematics> measurements =
1427 Collections.emptyList();
1428
1429 final Matrix ba = generateBa();
1430 final double biasX = ba.getElementAtIndex(0);
1431 final double biasY = ba.getElementAtIndex(1);
1432 final double biasZ = ba.getElementAtIndex(2);
1433
1434 final KnownGravityNormAccelerometerCalibrator calibrator =
1435 new KnownGravityNormAccelerometerCalibrator(measurements,
1436 true, biasX, biasY, biasZ);
1437
1438
1439 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1440 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1441 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1442 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1443 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1444 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445 final Acceleration bx2 = new Acceleration(0.0,
1446 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1447 calibrator.getInitialBiasXAsAcceleration(bx2);
1448 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1449 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1450 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1451 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1452 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1453 final Acceleration by2 = new Acceleration(0.0,
1454 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1455 calibrator.getInitialBiasYAsAcceleration(by2);
1456 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1457 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1458 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1459 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1460 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1461 final Acceleration bz2 = new Acceleration(0.0,
1462 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1463 calibrator.getInitialBiasZAsAcceleration(bz2);
1464 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1465 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1466 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1467 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1468 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1469 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1470 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1471 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1472 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1473 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1474 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1475 final double[] bias1 = calibrator.getInitialBias();
1476 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1477 final double[] bias2 = new double[3];
1478 calibrator.getInitialBias(bias2);
1479 assertArrayEquals(bias1, bias2, 0.0);
1480 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1481 assertEquals(b1, ba);
1482 final Matrix b2 = new Matrix(3, 1);
1483 calibrator.getInitialBiasAsMatrix(b2);
1484 assertEquals(b1, b2);
1485 final Matrix ma1 = calibrator.getInitialMa();
1486 assertEquals(ma1, new Matrix(3, 3));
1487 final Matrix ma2 = new Matrix(3, 3);
1488 calibrator.getInitialMa(ma2);
1489 assertEquals(ma1, ma2);
1490 assertSame(calibrator.getMeasurements(), measurements);
1491 assertTrue(calibrator.isCommonAxisUsed());
1492 assertNull(calibrator.getListener());
1493 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1494 assertFalse(calibrator.isReady());
1495 assertFalse(calibrator.isRunning());
1496 assertNull(calibrator.getEstimatedBiases());
1497 assertFalse(calibrator.getEstimatedBiases(null));
1498 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1499 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1500 assertNull(calibrator.getEstimatedBiasFx());
1501 assertNull(calibrator.getEstimatedBiasFy());
1502 assertNull(calibrator.getEstimatedBiasFz());
1503 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1504 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1505 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1506 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1507 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1508 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1509 assertNull(calibrator.getEstimatedMa());
1510 assertNull(calibrator.getEstimatedSx());
1511 assertNull(calibrator.getEstimatedSy());
1512 assertNull(calibrator.getEstimatedSz());
1513 assertNull(calibrator.getEstimatedMxy());
1514 assertNull(calibrator.getEstimatedMxz());
1515 assertNull(calibrator.getEstimatedMyx());
1516 assertNull(calibrator.getEstimatedMyz());
1517 assertNull(calibrator.getEstimatedMzx());
1518 assertNull(calibrator.getEstimatedMzy());
1519 assertNull(calibrator.getEstimatedCovariance());
1520 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1521 assertNull(calibrator.getGroundTruthGravityNorm());
1522 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1523 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1524 }
1525
1526 @Test
1527 public void testConstructor16() throws WrongSizeException {
1528 final Collection<StandardDeviationBodyKinematics> measurements =
1529 Collections.emptyList();
1530
1531 final Matrix ba = generateBa();
1532 final double biasX = ba.getElementAtIndex(0);
1533 final double biasY = ba.getElementAtIndex(1);
1534 final double biasZ = ba.getElementAtIndex(2);
1535
1536 final KnownGravityNormAccelerometerCalibrator calibrator =
1537 new KnownGravityNormAccelerometerCalibrator(measurements,
1538 true, biasX, biasY, biasZ, this);
1539
1540
1541 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1542 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1543 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1544 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1545 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1547 final Acceleration bx2 = new Acceleration(0.0,
1548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1549 calibrator.getInitialBiasXAsAcceleration(bx2);
1550 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1552 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1553 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1555 final Acceleration by2 = new Acceleration(0.0,
1556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1557 calibrator.getInitialBiasYAsAcceleration(by2);
1558 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1560 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1561 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1563 final Acceleration bz2 = new Acceleration(0.0,
1564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1565 calibrator.getInitialBiasZAsAcceleration(bz2);
1566 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1568 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1569 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1570 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1571 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1572 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1573 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1574 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1575 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1576 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1577 final double[] bias1 = calibrator.getInitialBias();
1578 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1579 final double[] bias2 = new double[3];
1580 calibrator.getInitialBias(bias2);
1581 assertArrayEquals(bias1, bias2, 0.0);
1582 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1583 assertEquals(b1, ba);
1584 final Matrix b2 = new Matrix(3, 1);
1585 calibrator.getInitialBiasAsMatrix(b2);
1586 assertEquals(b1, b2);
1587 final Matrix ma1 = calibrator.getInitialMa();
1588 assertEquals(ma1, new Matrix(3, 3));
1589 final Matrix ma2 = new Matrix(3, 3);
1590 calibrator.getInitialMa(ma2);
1591 assertEquals(ma1, ma2);
1592 assertSame(calibrator.getMeasurements(), measurements);
1593 assertTrue(calibrator.isCommonAxisUsed());
1594 assertSame(calibrator.getListener(), this);
1595 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1596 assertFalse(calibrator.isReady());
1597 assertFalse(calibrator.isRunning());
1598 assertNull(calibrator.getEstimatedBiases());
1599 assertFalse(calibrator.getEstimatedBiases(null));
1600 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1601 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1602 assertNull(calibrator.getEstimatedBiasFx());
1603 assertNull(calibrator.getEstimatedBiasFy());
1604 assertNull(calibrator.getEstimatedBiasFz());
1605 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1606 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1607 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1608 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1609 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1610 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1611 assertNull(calibrator.getEstimatedMa());
1612 assertNull(calibrator.getEstimatedSx());
1613 assertNull(calibrator.getEstimatedSy());
1614 assertNull(calibrator.getEstimatedSz());
1615 assertNull(calibrator.getEstimatedMxy());
1616 assertNull(calibrator.getEstimatedMxz());
1617 assertNull(calibrator.getEstimatedMyx());
1618 assertNull(calibrator.getEstimatedMyz());
1619 assertNull(calibrator.getEstimatedMzx());
1620 assertNull(calibrator.getEstimatedMzy());
1621 assertNull(calibrator.getEstimatedCovariance());
1622 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1623 assertNull(calibrator.getGroundTruthGravityNorm());
1624 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1625 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1626 }
1627
1628 @Test
1629 public void testConstructor17() throws WrongSizeException {
1630 final Matrix ba = generateBa();
1631 final double biasX = ba.getElementAtIndex(0);
1632 final double biasY = ba.getElementAtIndex(1);
1633 final double biasZ = ba.getElementAtIndex(2);
1634
1635 final Acceleration bx = new Acceleration(biasX,
1636 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1637 final Acceleration by = new Acceleration(biasY,
1638 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1639 final Acceleration bz = new Acceleration(biasZ,
1640 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1641
1642 final KnownGravityNormAccelerometerCalibrator calibrator =
1643 new KnownGravityNormAccelerometerCalibrator(bx, by, bz);
1644
1645
1646 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1647 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1648 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1649 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1650 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1651 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652 final Acceleration bx2 = new Acceleration(0.0,
1653 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1654 calibrator.getInitialBiasXAsAcceleration(bx2);
1655 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1656 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1657 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1658 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1659 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660 final Acceleration by2 = new Acceleration(0.0,
1661 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1662 calibrator.getInitialBiasYAsAcceleration(by2);
1663 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1664 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1665 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1666 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1667 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668 final Acceleration bz2 = new Acceleration(0.0,
1669 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1670 calibrator.getInitialBiasZAsAcceleration(bz2);
1671 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1672 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1673 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1674 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1675 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1676 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1677 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1678 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1679 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1680 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1681 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1682 final double[] bias1 = calibrator.getInitialBias();
1683 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1684 final double[] bias2 = new double[3];
1685 calibrator.getInitialBias(bias2);
1686 assertArrayEquals(bias1, bias2, 0.0);
1687 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1688 assertEquals(b1, ba);
1689 final Matrix b2 = new Matrix(3, 1);
1690 calibrator.getInitialBiasAsMatrix(b2);
1691 assertEquals(b1, b2);
1692 final Matrix ma1 = calibrator.getInitialMa();
1693 assertEquals(ma1, new Matrix(3, 3));
1694 final Matrix ma2 = new Matrix(3, 3);
1695 calibrator.getInitialMa(ma2);
1696 assertEquals(ma1, ma2);
1697 assertNull(calibrator.getMeasurements());
1698 assertFalse(calibrator.isCommonAxisUsed());
1699 assertNull(calibrator.getListener());
1700 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1701 assertFalse(calibrator.isReady());
1702 assertFalse(calibrator.isRunning());
1703 assertNull(calibrator.getEstimatedBiases());
1704 assertFalse(calibrator.getEstimatedBiases(null));
1705 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1706 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1707 assertNull(calibrator.getEstimatedBiasFx());
1708 assertNull(calibrator.getEstimatedBiasFy());
1709 assertNull(calibrator.getEstimatedBiasFz());
1710 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1711 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1712 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1713 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1714 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1715 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1716 assertNull(calibrator.getEstimatedMa());
1717 assertNull(calibrator.getEstimatedSx());
1718 assertNull(calibrator.getEstimatedSy());
1719 assertNull(calibrator.getEstimatedSz());
1720 assertNull(calibrator.getEstimatedMxy());
1721 assertNull(calibrator.getEstimatedMxz());
1722 assertNull(calibrator.getEstimatedMyx());
1723 assertNull(calibrator.getEstimatedMyz());
1724 assertNull(calibrator.getEstimatedMzx());
1725 assertNull(calibrator.getEstimatedMzy());
1726 assertNull(calibrator.getEstimatedCovariance());
1727 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1728 assertNull(calibrator.getGroundTruthGravityNorm());
1729 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1730 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1731 }
1732
1733 @Test
1734 public void testConstructor18() throws WrongSizeException {
1735 final Matrix ba = generateBa();
1736 final double biasX = ba.getElementAtIndex(0);
1737 final double biasY = ba.getElementAtIndex(1);
1738 final double biasZ = ba.getElementAtIndex(2);
1739
1740 final Acceleration bx = new Acceleration(biasX,
1741 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1742 final Acceleration by = new Acceleration(biasY,
1743 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1744 final Acceleration bz = new Acceleration(biasZ,
1745 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1746
1747 final KnownGravityNormAccelerometerCalibrator calibrator =
1748 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
1749 this);
1750
1751
1752 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1753 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1754 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1755 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1756 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1757 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1758 final Acceleration bx2 = new Acceleration(0.0,
1759 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1760 calibrator.getInitialBiasXAsAcceleration(bx2);
1761 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1762 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1763 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1764 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1765 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766 final Acceleration by2 = new Acceleration(0.0,
1767 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1768 calibrator.getInitialBiasYAsAcceleration(by2);
1769 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1770 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1771 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1772 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1773 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1774 final Acceleration bz2 = new Acceleration(0.0,
1775 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1776 calibrator.getInitialBiasZAsAcceleration(bz2);
1777 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1778 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1779 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1780 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1781 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1782 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1783 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1784 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1785 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1786 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1787 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1788 final double[] bias1 = calibrator.getInitialBias();
1789 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1790 final double[] bias2 = new double[3];
1791 calibrator.getInitialBias(bias2);
1792 assertArrayEquals(bias1, bias2, 0.0);
1793 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1794 assertEquals(b1, ba);
1795 final Matrix b2 = new Matrix(3, 1);
1796 calibrator.getInitialBiasAsMatrix(b2);
1797 assertEquals(b1, b2);
1798 final Matrix ma1 = calibrator.getInitialMa();
1799 assertEquals(ma1, new Matrix(3, 3));
1800 final Matrix ma2 = new Matrix(3, 3);
1801 calibrator.getInitialMa(ma2);
1802 assertEquals(ma1, ma2);
1803 assertNull(calibrator.getMeasurements());
1804 assertFalse(calibrator.isCommonAxisUsed());
1805 assertSame(calibrator.getListener(), this);
1806 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1807 assertFalse(calibrator.isReady());
1808 assertFalse(calibrator.isRunning());
1809 assertNull(calibrator.getEstimatedBiases());
1810 assertFalse(calibrator.getEstimatedBiases(null));
1811 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1812 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1813 assertNull(calibrator.getEstimatedBiasFx());
1814 assertNull(calibrator.getEstimatedBiasFy());
1815 assertNull(calibrator.getEstimatedBiasFz());
1816 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1817 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1818 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1819 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1820 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1821 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1822 assertNull(calibrator.getEstimatedMa());
1823 assertNull(calibrator.getEstimatedSx());
1824 assertNull(calibrator.getEstimatedSy());
1825 assertNull(calibrator.getEstimatedSz());
1826 assertNull(calibrator.getEstimatedMxy());
1827 assertNull(calibrator.getEstimatedMxz());
1828 assertNull(calibrator.getEstimatedMyx());
1829 assertNull(calibrator.getEstimatedMyz());
1830 assertNull(calibrator.getEstimatedMzx());
1831 assertNull(calibrator.getEstimatedMzy());
1832 assertNull(calibrator.getEstimatedCovariance());
1833 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1834 assertNull(calibrator.getGroundTruthGravityNorm());
1835 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1836 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1837 }
1838
1839 @Test
1840 public void testConstructor19() throws WrongSizeException {
1841 final Collection<StandardDeviationBodyKinematics> measurements =
1842 Collections.emptyList();
1843
1844 final Matrix ba = generateBa();
1845 final double biasX = ba.getElementAtIndex(0);
1846 final double biasY = ba.getElementAtIndex(1);
1847 final double biasZ = ba.getElementAtIndex(2);
1848
1849 final Acceleration bx = new Acceleration(biasX,
1850 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1851 final Acceleration by = new Acceleration(biasY,
1852 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1853 final Acceleration bz = new Acceleration(biasZ,
1854 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1855
1856 final KnownGravityNormAccelerometerCalibrator calibrator =
1857 new KnownGravityNormAccelerometerCalibrator(measurements,
1858 bx, by, bz);
1859
1860
1861 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1862 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1863 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1864 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1865 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1866 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867 final Acceleration bx2 = new Acceleration(0.0,
1868 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1869 calibrator.getInitialBiasXAsAcceleration(bx2);
1870 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1871 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1872 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1873 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1874 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1875 final Acceleration by2 = new Acceleration(0.0,
1876 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1877 calibrator.getInitialBiasYAsAcceleration(by2);
1878 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1879 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1880 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1881 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1882 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1883 final Acceleration bz2 = new Acceleration(0.0,
1884 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1885 calibrator.getInitialBiasZAsAcceleration(bz2);
1886 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1887 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1888 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1889 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1890 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1891 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1892 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1893 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1894 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1895 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1896 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1897 final double[] bias1 = calibrator.getInitialBias();
1898 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1899 final double[] bias2 = new double[3];
1900 calibrator.getInitialBias(bias2);
1901 assertArrayEquals(bias1, bias2, 0.0);
1902 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1903 assertEquals(b1, ba);
1904 final Matrix b2 = new Matrix(3, 1);
1905 calibrator.getInitialBiasAsMatrix(b2);
1906 assertEquals(b1, b2);
1907 final Matrix ma1 = calibrator.getInitialMa();
1908 assertEquals(ma1, new Matrix(3, 3));
1909 final Matrix ma2 = new Matrix(3, 3);
1910 calibrator.getInitialMa(ma2);
1911 assertEquals(ma1, ma2);
1912 assertSame(calibrator.getMeasurements(), measurements);
1913 assertFalse(calibrator.isCommonAxisUsed());
1914 assertNull(calibrator.getListener());
1915 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1916 assertFalse(calibrator.isReady());
1917 assertFalse(calibrator.isRunning());
1918 assertNull(calibrator.getEstimatedBiases());
1919 assertFalse(calibrator.getEstimatedBiases(null));
1920 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1921 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1922 assertNull(calibrator.getEstimatedBiasFx());
1923 assertNull(calibrator.getEstimatedBiasFy());
1924 assertNull(calibrator.getEstimatedBiasFz());
1925 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1926 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1927 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1928 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1929 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1930 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1931 assertNull(calibrator.getEstimatedMa());
1932 assertNull(calibrator.getEstimatedSx());
1933 assertNull(calibrator.getEstimatedSy());
1934 assertNull(calibrator.getEstimatedSz());
1935 assertNull(calibrator.getEstimatedMxy());
1936 assertNull(calibrator.getEstimatedMxz());
1937 assertNull(calibrator.getEstimatedMyx());
1938 assertNull(calibrator.getEstimatedMyz());
1939 assertNull(calibrator.getEstimatedMzx());
1940 assertNull(calibrator.getEstimatedMzy());
1941 assertNull(calibrator.getEstimatedCovariance());
1942 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1943 assertNull(calibrator.getGroundTruthGravityNorm());
1944 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1945 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1946 }
1947
1948 @Test
1949 public void testConstructor20() throws WrongSizeException {
1950 final Collection<StandardDeviationBodyKinematics> measurements =
1951 Collections.emptyList();
1952
1953 final Matrix ba = generateBa();
1954 final double biasX = ba.getElementAtIndex(0);
1955 final double biasY = ba.getElementAtIndex(1);
1956 final double biasZ = ba.getElementAtIndex(2);
1957
1958 final Acceleration bx = new Acceleration(biasX,
1959 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1960 final Acceleration by = new Acceleration(biasY,
1961 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1962 final Acceleration bz = new Acceleration(biasZ,
1963 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1964
1965 final KnownGravityNormAccelerometerCalibrator calibrator =
1966 new KnownGravityNormAccelerometerCalibrator(measurements,
1967 bx, by, bz, this);
1968
1969
1970 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1971 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1972 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1973 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1974 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1975 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1976 final Acceleration bx2 = new Acceleration(0.0,
1977 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1978 calibrator.getInitialBiasXAsAcceleration(bx2);
1979 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1980 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1981 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1982 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1983 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1984 final Acceleration by2 = new Acceleration(0.0,
1985 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1986 calibrator.getInitialBiasYAsAcceleration(by2);
1987 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1988 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1989 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1990 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1991 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1992 final Acceleration bz2 = new Acceleration(0.0,
1993 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1994 calibrator.getInitialBiasZAsAcceleration(bz2);
1995 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1996 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1997 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1998 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1999 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2000 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2001 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2002 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2003 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2004 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2005 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2006 final double[] bias1 = calibrator.getInitialBias();
2007 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2008 final double[] bias2 = new double[3];
2009 calibrator.getInitialBias(bias2);
2010 assertArrayEquals(bias1, bias2, 0.0);
2011 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2012 assertEquals(b1, ba);
2013 final Matrix b2 = new Matrix(3, 1);
2014 calibrator.getInitialBiasAsMatrix(b2);
2015 assertEquals(b1, b2);
2016 final Matrix ma1 = calibrator.getInitialMa();
2017 assertEquals(ma1, new Matrix(3, 3));
2018 final Matrix ma2 = new Matrix(3, 3);
2019 calibrator.getInitialMa(ma2);
2020 assertEquals(ma1, ma2);
2021 assertSame(calibrator.getMeasurements(), measurements);
2022 assertFalse(calibrator.isCommonAxisUsed());
2023 assertSame(calibrator.getListener(), this);
2024 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2025 assertFalse(calibrator.isReady());
2026 assertFalse(calibrator.isRunning());
2027 assertNull(calibrator.getEstimatedBiases());
2028 assertFalse(calibrator.getEstimatedBiases(null));
2029 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2030 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2031 assertNull(calibrator.getEstimatedBiasFx());
2032 assertNull(calibrator.getEstimatedBiasFy());
2033 assertNull(calibrator.getEstimatedBiasFz());
2034 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2035 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2036 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2037 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2038 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2039 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2040 assertNull(calibrator.getEstimatedMa());
2041 assertNull(calibrator.getEstimatedSx());
2042 assertNull(calibrator.getEstimatedSy());
2043 assertNull(calibrator.getEstimatedSz());
2044 assertNull(calibrator.getEstimatedMxy());
2045 assertNull(calibrator.getEstimatedMxz());
2046 assertNull(calibrator.getEstimatedMyx());
2047 assertNull(calibrator.getEstimatedMyz());
2048 assertNull(calibrator.getEstimatedMzx());
2049 assertNull(calibrator.getEstimatedMzy());
2050 assertNull(calibrator.getEstimatedCovariance());
2051 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2052 assertNull(calibrator.getGroundTruthGravityNorm());
2053 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2054 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2055 }
2056
2057 @Test
2058 public void testConstructor21() throws WrongSizeException {
2059 final Matrix ba = generateBa();
2060 final double biasX = ba.getElementAtIndex(0);
2061 final double biasY = ba.getElementAtIndex(1);
2062 final double biasZ = ba.getElementAtIndex(2);
2063
2064 final Acceleration bx = new Acceleration(biasX,
2065 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2066 final Acceleration by = new Acceleration(biasY,
2067 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2068 final Acceleration bz = new Acceleration(biasZ,
2069 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2070
2071 final KnownGravityNormAccelerometerCalibrator calibrator =
2072 new KnownGravityNormAccelerometerCalibrator(
2073 true, bx, by, bz);
2074
2075
2076 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2077 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2078 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2079 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2080 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2081 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2082 final Acceleration bx2 = new Acceleration(0.0,
2083 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2084 calibrator.getInitialBiasXAsAcceleration(bx2);
2085 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2086 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2087 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2088 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2089 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2090 final Acceleration by2 = new Acceleration(0.0,
2091 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2092 calibrator.getInitialBiasYAsAcceleration(by2);
2093 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2094 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2095 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2096 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2097 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2098 final Acceleration bz2 = new Acceleration(0.0,
2099 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2100 calibrator.getInitialBiasZAsAcceleration(bz2);
2101 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2102 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2103 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2104 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2105 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2106 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2107 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2108 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2109 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2110 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2111 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2112 final double[] bias1 = calibrator.getInitialBias();
2113 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2114 final double[] bias2 = new double[3];
2115 calibrator.getInitialBias(bias2);
2116 assertArrayEquals(bias1, bias2, 0.0);
2117 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2118 assertEquals(b1, ba);
2119 final Matrix b2 = new Matrix(3, 1);
2120 calibrator.getInitialBiasAsMatrix(b2);
2121 assertEquals(b1, b2);
2122 final Matrix ma1 = calibrator.getInitialMa();
2123 assertEquals(ma1, new Matrix(3, 3));
2124 final Matrix ma2 = new Matrix(3, 3);
2125 calibrator.getInitialMa(ma2);
2126 assertEquals(ma1, ma2);
2127 assertNull(calibrator.getMeasurements());
2128 assertTrue(calibrator.isCommonAxisUsed());
2129 assertNull(calibrator.getListener());
2130 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2131 assertFalse(calibrator.isReady());
2132 assertFalse(calibrator.isRunning());
2133 assertNull(calibrator.getEstimatedBiases());
2134 assertFalse(calibrator.getEstimatedBiases(null));
2135 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2136 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2137 assertNull(calibrator.getEstimatedBiasFx());
2138 assertNull(calibrator.getEstimatedBiasFy());
2139 assertNull(calibrator.getEstimatedBiasFz());
2140 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2141 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2142 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2143 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2144 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2145 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2146 assertNull(calibrator.getEstimatedMa());
2147 assertNull(calibrator.getEstimatedSx());
2148 assertNull(calibrator.getEstimatedSy());
2149 assertNull(calibrator.getEstimatedSz());
2150 assertNull(calibrator.getEstimatedMxy());
2151 assertNull(calibrator.getEstimatedMxz());
2152 assertNull(calibrator.getEstimatedMyx());
2153 assertNull(calibrator.getEstimatedMyz());
2154 assertNull(calibrator.getEstimatedMzx());
2155 assertNull(calibrator.getEstimatedMzy());
2156 assertNull(calibrator.getEstimatedCovariance());
2157 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2158 assertNull(calibrator.getGroundTruthGravityNorm());
2159 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2160 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2161 }
2162
2163 @Test
2164 public void testConstructor22() throws WrongSizeException {
2165 final Matrix ba = generateBa();
2166 final double biasX = ba.getElementAtIndex(0);
2167 final double biasY = ba.getElementAtIndex(1);
2168 final double biasZ = ba.getElementAtIndex(2);
2169
2170 final Acceleration bx = new Acceleration(biasX,
2171 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2172 final Acceleration by = new Acceleration(biasY,
2173 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2174 final Acceleration bz = new Acceleration(biasZ,
2175 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2176
2177 final KnownGravityNormAccelerometerCalibrator calibrator =
2178 new KnownGravityNormAccelerometerCalibrator(
2179 true, bx, by, bz, this);
2180
2181
2182 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2183 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2184 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2185 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2186 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2187 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2188 final Acceleration bx2 = new Acceleration(0.0,
2189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2190 calibrator.getInitialBiasXAsAcceleration(bx2);
2191 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2192 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2193 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2194 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2195 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2196 final Acceleration by2 = new Acceleration(0.0,
2197 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2198 calibrator.getInitialBiasYAsAcceleration(by2);
2199 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2200 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2201 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2202 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2203 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2204 final Acceleration bz2 = new Acceleration(0.0,
2205 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2206 calibrator.getInitialBiasZAsAcceleration(bz2);
2207 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2208 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2209 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2210 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2211 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2212 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2213 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2214 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2215 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2216 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2217 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2218 final double[] bias1 = calibrator.getInitialBias();
2219 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2220 final double[] bias2 = new double[3];
2221 calibrator.getInitialBias(bias2);
2222 assertArrayEquals(bias1, bias2, 0.0);
2223 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2224 assertEquals(b1, ba);
2225 final Matrix b2 = new Matrix(3, 1);
2226 calibrator.getInitialBiasAsMatrix(b2);
2227 assertEquals(b1, b2);
2228 final Matrix ma1 = calibrator.getInitialMa();
2229 assertEquals(ma1, new Matrix(3, 3));
2230 final Matrix ma2 = new Matrix(3, 3);
2231 calibrator.getInitialMa(ma2);
2232 assertEquals(ma1, ma2);
2233 assertNull(calibrator.getMeasurements());
2234 assertTrue(calibrator.isCommonAxisUsed());
2235 assertSame(calibrator.getListener(), this);
2236 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2237 assertFalse(calibrator.isReady());
2238 assertFalse(calibrator.isRunning());
2239 assertNull(calibrator.getEstimatedBiases());
2240 assertFalse(calibrator.getEstimatedBiases(null));
2241 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2242 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2243 assertNull(calibrator.getEstimatedBiasFx());
2244 assertNull(calibrator.getEstimatedBiasFy());
2245 assertNull(calibrator.getEstimatedBiasFz());
2246 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2247 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2248 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2249 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2250 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2251 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2252 assertNull(calibrator.getEstimatedMa());
2253 assertNull(calibrator.getEstimatedSx());
2254 assertNull(calibrator.getEstimatedSy());
2255 assertNull(calibrator.getEstimatedSz());
2256 assertNull(calibrator.getEstimatedMxy());
2257 assertNull(calibrator.getEstimatedMxz());
2258 assertNull(calibrator.getEstimatedMyx());
2259 assertNull(calibrator.getEstimatedMyz());
2260 assertNull(calibrator.getEstimatedMzx());
2261 assertNull(calibrator.getEstimatedMzy());
2262 assertNull(calibrator.getEstimatedCovariance());
2263 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2264 assertNull(calibrator.getGroundTruthGravityNorm());
2265 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2266 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2267 }
2268
2269 @Test
2270 public void testConstructor23() throws WrongSizeException {
2271 final Collection<StandardDeviationBodyKinematics> measurements =
2272 Collections.emptyList();
2273
2274 final Matrix ba = generateBa();
2275 final double biasX = ba.getElementAtIndex(0);
2276 final double biasY = ba.getElementAtIndex(1);
2277 final double biasZ = ba.getElementAtIndex(2);
2278
2279 final Acceleration bx = new Acceleration(biasX,
2280 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2281 final Acceleration by = new Acceleration(biasY,
2282 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2283 final Acceleration bz = new Acceleration(biasZ,
2284 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2285
2286 final KnownGravityNormAccelerometerCalibrator calibrator =
2287 new KnownGravityNormAccelerometerCalibrator(measurements,
2288 true, bx, by, bz);
2289
2290
2291 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2292 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2293 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2294 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2295 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2296 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2297 final Acceleration bx2 = new Acceleration(0.0,
2298 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2299 calibrator.getInitialBiasXAsAcceleration(bx2);
2300 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2301 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2302 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2303 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2304 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2305 final Acceleration by2 = new Acceleration(0.0,
2306 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2307 calibrator.getInitialBiasYAsAcceleration(by2);
2308 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2309 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2310 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2311 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2312 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2313 final Acceleration bz2 = new Acceleration(0.0,
2314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2315 calibrator.getInitialBiasZAsAcceleration(bz2);
2316 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2317 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2318 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2319 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2320 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2321 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2322 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2323 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2324 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2325 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2326 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2327 final double[] bias1 = calibrator.getInitialBias();
2328 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2329 final double[] bias2 = new double[3];
2330 calibrator.getInitialBias(bias2);
2331 assertArrayEquals(bias1, bias2, 0.0);
2332 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2333 assertEquals(b1, ba);
2334 final Matrix b2 = new Matrix(3, 1);
2335 calibrator.getInitialBiasAsMatrix(b2);
2336 assertEquals(b1, b2);
2337 final Matrix ma1 = calibrator.getInitialMa();
2338 assertEquals(ma1, new Matrix(3, 3));
2339 final Matrix ma2 = new Matrix(3, 3);
2340 calibrator.getInitialMa(ma2);
2341 assertEquals(ma1, ma2);
2342 assertSame(calibrator.getMeasurements(), measurements);
2343 assertTrue(calibrator.isCommonAxisUsed());
2344 assertNull(calibrator.getListener());
2345 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2346 assertFalse(calibrator.isReady());
2347 assertFalse(calibrator.isRunning());
2348 assertNull(calibrator.getEstimatedBiases());
2349 assertFalse(calibrator.getEstimatedBiases(null));
2350 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2351 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2352 assertNull(calibrator.getEstimatedBiasFx());
2353 assertNull(calibrator.getEstimatedBiasFy());
2354 assertNull(calibrator.getEstimatedBiasFz());
2355 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2356 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2357 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2358 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2359 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2360 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2361 assertNull(calibrator.getEstimatedMa());
2362 assertNull(calibrator.getEstimatedSx());
2363 assertNull(calibrator.getEstimatedSy());
2364 assertNull(calibrator.getEstimatedSz());
2365 assertNull(calibrator.getEstimatedMxy());
2366 assertNull(calibrator.getEstimatedMxz());
2367 assertNull(calibrator.getEstimatedMyx());
2368 assertNull(calibrator.getEstimatedMyz());
2369 assertNull(calibrator.getEstimatedMzx());
2370 assertNull(calibrator.getEstimatedMzy());
2371 assertNull(calibrator.getEstimatedCovariance());
2372 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2373 assertNull(calibrator.getGroundTruthGravityNorm());
2374 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2375 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2376 }
2377
2378 @Test
2379 public void testConstructor24() throws WrongSizeException {
2380 final Collection<StandardDeviationBodyKinematics> measurements =
2381 Collections.emptyList();
2382
2383 final Matrix ba = generateBa();
2384 final double biasX = ba.getElementAtIndex(0);
2385 final double biasY = ba.getElementAtIndex(1);
2386 final double biasZ = ba.getElementAtIndex(2);
2387
2388 final Acceleration bx = new Acceleration(biasX,
2389 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2390 final Acceleration by = new Acceleration(biasY,
2391 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2392 final Acceleration bz = new Acceleration(biasZ,
2393 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2394
2395 final KnownGravityNormAccelerometerCalibrator calibrator =
2396 new KnownGravityNormAccelerometerCalibrator(measurements,
2397 true, bx, by, bz, this);
2398
2399
2400 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2401 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2402 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2403 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2404 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2405 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406 final Acceleration bx2 = new Acceleration(0.0,
2407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2408 calibrator.getInitialBiasXAsAcceleration(bx2);
2409 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2410 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2411 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2412 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2413 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414 final Acceleration by2 = new Acceleration(0.0,
2415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2416 calibrator.getInitialBiasYAsAcceleration(by2);
2417 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2418 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2419 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2420 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2421 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422 final Acceleration bz2 = new Acceleration(0.0,
2423 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2424 calibrator.getInitialBiasZAsAcceleration(bz2);
2425 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2426 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2427 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2428 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2429 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2430 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2431 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2432 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2433 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2434 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2435 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2436 final double[] bias1 = calibrator.getInitialBias();
2437 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2438 final double[] bias2 = new double[3];
2439 calibrator.getInitialBias(bias2);
2440 assertArrayEquals(bias1, bias2, 0.0);
2441 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2442 assertEquals(b1, ba);
2443 final Matrix b2 = new Matrix(3, 1);
2444 calibrator.getInitialBiasAsMatrix(b2);
2445 assertEquals(b1, b2);
2446 final Matrix ma1 = calibrator.getInitialMa();
2447 assertEquals(ma1, new Matrix(3, 3));
2448 final Matrix ma2 = new Matrix(3, 3);
2449 calibrator.getInitialMa(ma2);
2450 assertEquals(ma1, ma2);
2451 assertSame(calibrator.getMeasurements(), measurements);
2452 assertTrue(calibrator.isCommonAxisUsed());
2453 assertSame(calibrator.getListener(), this);
2454 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2455 assertFalse(calibrator.isReady());
2456 assertFalse(calibrator.isRunning());
2457 assertNull(calibrator.getEstimatedBiases());
2458 assertFalse(calibrator.getEstimatedBiases(null));
2459 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2460 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2461 assertNull(calibrator.getEstimatedBiasFx());
2462 assertNull(calibrator.getEstimatedBiasFy());
2463 assertNull(calibrator.getEstimatedBiasFz());
2464 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2465 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2466 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2467 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2468 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2469 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2470 assertNull(calibrator.getEstimatedMa());
2471 assertNull(calibrator.getEstimatedSx());
2472 assertNull(calibrator.getEstimatedSy());
2473 assertNull(calibrator.getEstimatedSz());
2474 assertNull(calibrator.getEstimatedMxy());
2475 assertNull(calibrator.getEstimatedMxz());
2476 assertNull(calibrator.getEstimatedMyx());
2477 assertNull(calibrator.getEstimatedMyz());
2478 assertNull(calibrator.getEstimatedMzx());
2479 assertNull(calibrator.getEstimatedMzy());
2480 assertNull(calibrator.getEstimatedCovariance());
2481 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2482 assertNull(calibrator.getGroundTruthGravityNorm());
2483 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2484 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2485 }
2486
2487 @Test
2488 public void testConstructor25() throws WrongSizeException {
2489 final Matrix ba = generateBa();
2490 final double biasX = ba.getElementAtIndex(0);
2491 final double biasY = ba.getElementAtIndex(1);
2492 final double biasZ = ba.getElementAtIndex(2);
2493
2494 final Matrix ma = generateMaCommonAxis();
2495 final double sx = ma.getElementAt(0, 0);
2496 final double sy = ma.getElementAt(1, 1);
2497 final double sz = ma.getElementAt(2, 2);
2498
2499 final KnownGravityNormAccelerometerCalibrator calibrator =
2500 new KnownGravityNormAccelerometerCalibrator(
2501 biasX, biasY, biasZ,
2502 sx, sy, sz);
2503
2504
2505 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2506 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2507 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2508 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2509 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2510 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2511 final Acceleration bx2 = new Acceleration(0.0,
2512 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2513 calibrator.getInitialBiasXAsAcceleration(bx2);
2514 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2515 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2516 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2517 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2518 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2519 final Acceleration by2 = new Acceleration(0.0,
2520 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2521 calibrator.getInitialBiasYAsAcceleration(by2);
2522 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2523 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2524 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2525 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2526 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2527 final Acceleration bz2 = new Acceleration(0.0,
2528 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2529 calibrator.getInitialBiasZAsAcceleration(bz2);
2530 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2531 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2532 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2533 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2534 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2535 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2536 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2537 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2538 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2539 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2540 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2541 final double[] bias1 = calibrator.getInitialBias();
2542 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2543 final double[] bias2 = new double[3];
2544 calibrator.getInitialBias(bias2);
2545 assertArrayEquals(bias1, bias2, 0.0);
2546 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2547 assertEquals(b1, ba);
2548 final Matrix b2 = new Matrix(3, 1);
2549 calibrator.getInitialBiasAsMatrix(b2);
2550 assertEquals(b1, b2);
2551 final Matrix ma1 = calibrator.getInitialMa();
2552 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2553 final Matrix ma2 = new Matrix(3, 3);
2554 calibrator.getInitialMa(ma2);
2555 assertEquals(ma1, ma2);
2556 assertNull(calibrator.getMeasurements());
2557 assertFalse(calibrator.isCommonAxisUsed());
2558 assertNull(calibrator.getListener());
2559 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2560 assertFalse(calibrator.isReady());
2561 assertFalse(calibrator.isRunning());
2562 assertNull(calibrator.getEstimatedBiases());
2563 assertFalse(calibrator.getEstimatedBiases(null));
2564 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2565 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2566 assertNull(calibrator.getEstimatedBiasFx());
2567 assertNull(calibrator.getEstimatedBiasFy());
2568 assertNull(calibrator.getEstimatedBiasFz());
2569 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2570 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2571 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2572 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2573 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2574 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2575 assertNull(calibrator.getEstimatedMa());
2576 assertNull(calibrator.getEstimatedSx());
2577 assertNull(calibrator.getEstimatedSy());
2578 assertNull(calibrator.getEstimatedSz());
2579 assertNull(calibrator.getEstimatedMxy());
2580 assertNull(calibrator.getEstimatedMxz());
2581 assertNull(calibrator.getEstimatedMyx());
2582 assertNull(calibrator.getEstimatedMyz());
2583 assertNull(calibrator.getEstimatedMzx());
2584 assertNull(calibrator.getEstimatedMzy());
2585 assertNull(calibrator.getEstimatedCovariance());
2586 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2587 assertNull(calibrator.getGroundTruthGravityNorm());
2588 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2589 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2590 }
2591
2592 @Test
2593 public void testConstructor26() throws WrongSizeException {
2594 final Collection<StandardDeviationBodyKinematics> measurements =
2595 Collections.emptyList();
2596
2597 final Matrix ba = generateBa();
2598 final double biasX = ba.getElementAtIndex(0);
2599 final double biasY = ba.getElementAtIndex(1);
2600 final double biasZ = ba.getElementAtIndex(2);
2601
2602 final Matrix ma = generateMaCommonAxis();
2603 final double sx = ma.getElementAt(0, 0);
2604 final double sy = ma.getElementAt(1, 1);
2605 final double sz = ma.getElementAt(2, 2);
2606
2607 final KnownGravityNormAccelerometerCalibrator calibrator =
2608 new KnownGravityNormAccelerometerCalibrator(measurements,
2609 biasX, biasY, biasZ, sx, sy, sz);
2610
2611
2612 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2613 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2614 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2615 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2616 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2617 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2618 final Acceleration bx2 = new Acceleration(0.0,
2619 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2620 calibrator.getInitialBiasXAsAcceleration(bx2);
2621 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2622 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2623 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2624 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2625 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2626 final Acceleration by2 = new Acceleration(0.0,
2627 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2628 calibrator.getInitialBiasYAsAcceleration(by2);
2629 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2630 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2631 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2632 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2633 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2634 final Acceleration bz2 = new Acceleration(0.0,
2635 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2636 calibrator.getInitialBiasZAsAcceleration(bz2);
2637 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2638 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2639 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2640 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2641 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2642 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2643 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2644 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2645 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2646 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2647 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2648 final double[] bias1 = calibrator.getInitialBias();
2649 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2650 final double[] bias2 = new double[3];
2651 calibrator.getInitialBias(bias2);
2652 assertArrayEquals(bias1, bias2, 0.0);
2653 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2654 assertEquals(b1, ba);
2655 final Matrix b2 = new Matrix(3, 1);
2656 calibrator.getInitialBiasAsMatrix(b2);
2657 assertEquals(b1, b2);
2658 final Matrix ma1 = calibrator.getInitialMa();
2659 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2660 final Matrix ma2 = new Matrix(3, 3);
2661 calibrator.getInitialMa(ma2);
2662 assertEquals(ma1, ma2);
2663 assertSame(calibrator.getMeasurements(), measurements);
2664 assertFalse(calibrator.isCommonAxisUsed());
2665 assertNull(calibrator.getListener());
2666 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2667 assertFalse(calibrator.isReady());
2668 assertFalse(calibrator.isRunning());
2669 assertNull(calibrator.getEstimatedBiases());
2670 assertFalse(calibrator.getEstimatedBiases(null));
2671 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2672 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2673 assertNull(calibrator.getEstimatedBiasFx());
2674 assertNull(calibrator.getEstimatedBiasFy());
2675 assertNull(calibrator.getEstimatedBiasFz());
2676 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2677 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2678 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2679 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2680 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2681 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2682 assertNull(calibrator.getEstimatedMa());
2683 assertNull(calibrator.getEstimatedSx());
2684 assertNull(calibrator.getEstimatedSy());
2685 assertNull(calibrator.getEstimatedSz());
2686 assertNull(calibrator.getEstimatedMxy());
2687 assertNull(calibrator.getEstimatedMxz());
2688 assertNull(calibrator.getEstimatedMyx());
2689 assertNull(calibrator.getEstimatedMyz());
2690 assertNull(calibrator.getEstimatedMzx());
2691 assertNull(calibrator.getEstimatedMzy());
2692 assertNull(calibrator.getEstimatedCovariance());
2693 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2694 assertNull(calibrator.getGroundTruthGravityNorm());
2695 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2696 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2697 }
2698
2699 @Test
2700 public void testConstructor27() throws WrongSizeException {
2701 final Collection<StandardDeviationBodyKinematics> measurements =
2702 Collections.emptyList();
2703
2704 final Matrix ba = generateBa();
2705 final double biasX = ba.getElementAtIndex(0);
2706 final double biasY = ba.getElementAtIndex(1);
2707 final double biasZ = ba.getElementAtIndex(2);
2708
2709 final Matrix ma = generateMaCommonAxis();
2710 final double sx = ma.getElementAt(0, 0);
2711 final double sy = ma.getElementAt(1, 1);
2712 final double sz = ma.getElementAt(2, 2);
2713
2714 final KnownGravityNormAccelerometerCalibrator calibrator =
2715 new KnownGravityNormAccelerometerCalibrator(measurements,
2716 biasX, biasY, biasZ, sx, sy, sz, this);
2717
2718
2719 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2720 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2721 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2722 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2723 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2724 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2725 final Acceleration bx2 = new Acceleration(0.0,
2726 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2727 calibrator.getInitialBiasXAsAcceleration(bx2);
2728 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2729 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2730 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2731 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2732 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2733 final Acceleration by2 = new Acceleration(0.0,
2734 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2735 calibrator.getInitialBiasYAsAcceleration(by2);
2736 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2737 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2738 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2739 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2740 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2741 final Acceleration bz2 = new Acceleration(0.0,
2742 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2743 calibrator.getInitialBiasZAsAcceleration(bz2);
2744 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2745 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2746 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2747 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2748 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2749 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2750 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2751 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2752 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2753 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2754 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2755 final double[] bias1 = calibrator.getInitialBias();
2756 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2757 final double[] bias2 = new double[3];
2758 calibrator.getInitialBias(bias2);
2759 assertArrayEquals(bias1, bias2, 0.0);
2760 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2761 assertEquals(b1, ba);
2762 final Matrix b2 = new Matrix(3, 1);
2763 calibrator.getInitialBiasAsMatrix(b2);
2764 assertEquals(b1, b2);
2765 final Matrix ma1 = calibrator.getInitialMa();
2766 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2767 final Matrix ma2 = new Matrix(3, 3);
2768 calibrator.getInitialMa(ma2);
2769 assertEquals(ma1, ma2);
2770 assertSame(calibrator.getMeasurements(), measurements);
2771 assertFalse(calibrator.isCommonAxisUsed());
2772 assertSame(calibrator.getListener(), this);
2773 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2774 assertFalse(calibrator.isReady());
2775 assertFalse(calibrator.isRunning());
2776 assertNull(calibrator.getEstimatedBiases());
2777 assertFalse(calibrator.getEstimatedBiases(null));
2778 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2779 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2780 assertNull(calibrator.getEstimatedBiasFx());
2781 assertNull(calibrator.getEstimatedBiasFy());
2782 assertNull(calibrator.getEstimatedBiasFz());
2783 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2784 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2785 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2786 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2787 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2788 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2789 assertNull(calibrator.getEstimatedMa());
2790 assertNull(calibrator.getEstimatedSx());
2791 assertNull(calibrator.getEstimatedSy());
2792 assertNull(calibrator.getEstimatedSz());
2793 assertNull(calibrator.getEstimatedMxy());
2794 assertNull(calibrator.getEstimatedMxz());
2795 assertNull(calibrator.getEstimatedMyx());
2796 assertNull(calibrator.getEstimatedMyz());
2797 assertNull(calibrator.getEstimatedMzx());
2798 assertNull(calibrator.getEstimatedMzy());
2799 assertNull(calibrator.getEstimatedCovariance());
2800 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2801 assertNull(calibrator.getGroundTruthGravityNorm());
2802 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2803 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2804 }
2805
2806 @Test
2807 public void testConstructor28() throws WrongSizeException {
2808 final Matrix ba = generateBa();
2809 final double biasX = ba.getElementAtIndex(0);
2810 final double biasY = ba.getElementAtIndex(1);
2811 final double biasZ = ba.getElementAtIndex(2);
2812
2813 final Matrix ma = generateMaCommonAxis();
2814 final double sx = ma.getElementAt(0, 0);
2815 final double sy = ma.getElementAt(1, 1);
2816 final double sz = ma.getElementAt(2, 2);
2817
2818 final KnownGravityNormAccelerometerCalibrator calibrator =
2819 new KnownGravityNormAccelerometerCalibrator(
2820 true, biasX, biasY, biasZ, sx, sy, sz);
2821
2822
2823 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2824 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2825 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2826 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2827 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2828 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2829 final Acceleration bx2 = new Acceleration(0.0,
2830 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2831 calibrator.getInitialBiasXAsAcceleration(bx2);
2832 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2833 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2834 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2835 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2836 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2837 final Acceleration by2 = new Acceleration(0.0,
2838 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2839 calibrator.getInitialBiasYAsAcceleration(by2);
2840 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2841 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2842 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2843 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2844 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2845 final Acceleration bz2 = new Acceleration(0.0,
2846 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2847 calibrator.getInitialBiasZAsAcceleration(bz2);
2848 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2849 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2850 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2851 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2852 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2853 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2854 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2855 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2856 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2857 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2858 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2859 final double[] bias1 = calibrator.getInitialBias();
2860 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2861 final double[] bias2 = new double[3];
2862 calibrator.getInitialBias(bias2);
2863 assertArrayEquals(bias1, bias2, 0.0);
2864 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2865 assertEquals(b1, ba);
2866 final Matrix b2 = new Matrix(3, 1);
2867 calibrator.getInitialBiasAsMatrix(b2);
2868 assertEquals(b1, b2);
2869 final Matrix ma1 = calibrator.getInitialMa();
2870 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2871 final Matrix ma2 = new Matrix(3, 3);
2872 calibrator.getInitialMa(ma2);
2873 assertEquals(ma1, ma2);
2874 assertNull(calibrator.getMeasurements());
2875 assertTrue(calibrator.isCommonAxisUsed());
2876 assertNull(calibrator.getListener());
2877 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2878 assertFalse(calibrator.isReady());
2879 assertFalse(calibrator.isRunning());
2880 assertNull(calibrator.getEstimatedBiases());
2881 assertFalse(calibrator.getEstimatedBiases(null));
2882 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2883 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2884 assertNull(calibrator.getEstimatedBiasFx());
2885 assertNull(calibrator.getEstimatedBiasFy());
2886 assertNull(calibrator.getEstimatedBiasFz());
2887 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2888 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2889 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2890 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2891 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2892 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2893 assertNull(calibrator.getEstimatedMa());
2894 assertNull(calibrator.getEstimatedSx());
2895 assertNull(calibrator.getEstimatedSy());
2896 assertNull(calibrator.getEstimatedSz());
2897 assertNull(calibrator.getEstimatedMxy());
2898 assertNull(calibrator.getEstimatedMxz());
2899 assertNull(calibrator.getEstimatedMyx());
2900 assertNull(calibrator.getEstimatedMyz());
2901 assertNull(calibrator.getEstimatedMzx());
2902 assertNull(calibrator.getEstimatedMzy());
2903 assertNull(calibrator.getEstimatedCovariance());
2904 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2905 assertNull(calibrator.getGroundTruthGravityNorm());
2906 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2907 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2908 }
2909
2910 @Test
2911 public void testConstructor29() throws WrongSizeException {
2912 final Matrix ba = generateBa();
2913 final double biasX = ba.getElementAtIndex(0);
2914 final double biasY = ba.getElementAtIndex(1);
2915 final double biasZ = ba.getElementAtIndex(2);
2916
2917 final Matrix ma = generateMaCommonAxis();
2918 final double sx = ma.getElementAt(0, 0);
2919 final double sy = ma.getElementAt(1, 1);
2920 final double sz = ma.getElementAt(2, 2);
2921
2922 final KnownGravityNormAccelerometerCalibrator calibrator =
2923 new KnownGravityNormAccelerometerCalibrator(
2924 true, biasX, biasY, biasZ,
2925 sx, sy, sz, this);
2926
2927
2928 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2929 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2930 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2931 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2932 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2933 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2934 final Acceleration bx2 = new Acceleration(0.0,
2935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2936 calibrator.getInitialBiasXAsAcceleration(bx2);
2937 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2938 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2939 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2940 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2941 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2942 final Acceleration by2 = new Acceleration(0.0,
2943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2944 calibrator.getInitialBiasYAsAcceleration(by2);
2945 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2946 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2947 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2948 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2949 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2950 final Acceleration bz2 = new Acceleration(0.0,
2951 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2952 calibrator.getInitialBiasZAsAcceleration(bz2);
2953 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2954 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2955 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2956 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2957 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2958 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2959 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2960 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2961 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2962 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2963 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2964 final double[] bias1 = calibrator.getInitialBias();
2965 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2966 final double[] bias2 = new double[3];
2967 calibrator.getInitialBias(bias2);
2968 assertArrayEquals(bias1, bias2, 0.0);
2969 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2970 assertEquals(b1, ba);
2971 final Matrix b2 = new Matrix(3, 1);
2972 calibrator.getInitialBiasAsMatrix(b2);
2973 assertEquals(b1, b2);
2974 final Matrix ma1 = calibrator.getInitialMa();
2975 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2976 final Matrix ma2 = new Matrix(3, 3);
2977 calibrator.getInitialMa(ma2);
2978 assertEquals(ma1, ma2);
2979 assertNull(calibrator.getMeasurements());
2980 assertTrue(calibrator.isCommonAxisUsed());
2981 assertSame(calibrator.getListener(), this);
2982 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2983 assertFalse(calibrator.isReady());
2984 assertFalse(calibrator.isRunning());
2985 assertNull(calibrator.getEstimatedBiases());
2986 assertFalse(calibrator.getEstimatedBiases(null));
2987 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2988 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2989 assertNull(calibrator.getEstimatedBiasFx());
2990 assertNull(calibrator.getEstimatedBiasFy());
2991 assertNull(calibrator.getEstimatedBiasFz());
2992 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2993 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2994 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2995 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2996 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2997 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2998 assertNull(calibrator.getEstimatedMa());
2999 assertNull(calibrator.getEstimatedSx());
3000 assertNull(calibrator.getEstimatedSy());
3001 assertNull(calibrator.getEstimatedSz());
3002 assertNull(calibrator.getEstimatedMxy());
3003 assertNull(calibrator.getEstimatedMxz());
3004 assertNull(calibrator.getEstimatedMyx());
3005 assertNull(calibrator.getEstimatedMyz());
3006 assertNull(calibrator.getEstimatedMzx());
3007 assertNull(calibrator.getEstimatedMzy());
3008 assertNull(calibrator.getEstimatedCovariance());
3009 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3010 assertNull(calibrator.getGroundTruthGravityNorm());
3011 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3012 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3013 }
3014
3015 @Test
3016 public void testConstructor30() throws WrongSizeException {
3017 final Collection<StandardDeviationBodyKinematics> measurements =
3018 Collections.emptyList();
3019 final Matrix ba = generateBa();
3020 final double biasX = ba.getElementAtIndex(0);
3021 final double biasY = ba.getElementAtIndex(1);
3022 final double biasZ = ba.getElementAtIndex(2);
3023
3024 final Matrix ma = generateMaCommonAxis();
3025 final double sx = ma.getElementAt(0, 0);
3026 final double sy = ma.getElementAt(1, 1);
3027 final double sz = ma.getElementAt(2, 2);
3028
3029 final KnownGravityNormAccelerometerCalibrator calibrator =
3030 new KnownGravityNormAccelerometerCalibrator(measurements,
3031 true, biasX, biasY, biasZ,
3032 sx, sy, sz);
3033
3034
3035 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3036 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3037 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3038 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3039 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3040 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3041 final Acceleration bx2 = new Acceleration(0.0,
3042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3043 calibrator.getInitialBiasXAsAcceleration(bx2);
3044 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3045 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3047 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3048 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3049 final Acceleration by2 = new Acceleration(0.0,
3050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3051 calibrator.getInitialBiasYAsAcceleration(by2);
3052 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3053 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3054 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3055 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3056 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3057 final Acceleration bz2 = new Acceleration(0.0,
3058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3059 calibrator.getInitialBiasZAsAcceleration(bz2);
3060 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3061 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3062 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3063 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3064 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3065 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3066 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3067 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3068 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3069 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3070 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3071 final double[] bias1 = calibrator.getInitialBias();
3072 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3073 final double[] bias2 = new double[3];
3074 calibrator.getInitialBias(bias2);
3075 assertArrayEquals(bias1, bias2, 0.0);
3076 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3077 assertEquals(b1, ba);
3078 final Matrix b2 = new Matrix(3, 1);
3079 calibrator.getInitialBiasAsMatrix(b2);
3080 assertEquals(b1, b2);
3081 final Matrix ma1 = calibrator.getInitialMa();
3082 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3083 final Matrix ma2 = new Matrix(3, 3);
3084 calibrator.getInitialMa(ma2);
3085 assertEquals(ma1, ma2);
3086 assertSame(calibrator.getMeasurements(), measurements);
3087 assertTrue(calibrator.isCommonAxisUsed());
3088 assertNull(calibrator.getListener());
3089 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3090 assertFalse(calibrator.isReady());
3091 assertFalse(calibrator.isRunning());
3092 assertNull(calibrator.getEstimatedBiases());
3093 assertFalse(calibrator.getEstimatedBiases(null));
3094 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3095 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3096 assertNull(calibrator.getEstimatedBiasFx());
3097 assertNull(calibrator.getEstimatedBiasFy());
3098 assertNull(calibrator.getEstimatedBiasFz());
3099 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3100 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3101 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3102 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3103 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3104 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3105 assertNull(calibrator.getEstimatedMa());
3106 assertNull(calibrator.getEstimatedSx());
3107 assertNull(calibrator.getEstimatedSy());
3108 assertNull(calibrator.getEstimatedSz());
3109 assertNull(calibrator.getEstimatedMxy());
3110 assertNull(calibrator.getEstimatedMxz());
3111 assertNull(calibrator.getEstimatedMyx());
3112 assertNull(calibrator.getEstimatedMyz());
3113 assertNull(calibrator.getEstimatedMzx());
3114 assertNull(calibrator.getEstimatedMzy());
3115 assertNull(calibrator.getEstimatedCovariance());
3116 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3117 assertNull(calibrator.getGroundTruthGravityNorm());
3118 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3119 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3120 }
3121
3122 @Test
3123 public void testConstructor31() throws WrongSizeException {
3124 final Collection<StandardDeviationBodyKinematics> measurements =
3125 Collections.emptyList();
3126 final Matrix ba = generateBa();
3127 final double biasX = ba.getElementAtIndex(0);
3128 final double biasY = ba.getElementAtIndex(1);
3129 final double biasZ = ba.getElementAtIndex(2);
3130
3131 final Matrix ma = generateMaCommonAxis();
3132 final double sx = ma.getElementAt(0, 0);
3133 final double sy = ma.getElementAt(1, 1);
3134 final double sz = ma.getElementAt(2, 2);
3135
3136 final KnownGravityNormAccelerometerCalibrator calibrator =
3137 new KnownGravityNormAccelerometerCalibrator(measurements,
3138 true, biasX, biasY, biasZ,
3139 sx, sy, sz, this);
3140
3141
3142 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3143 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3144 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3145 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3146 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3147 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3148 final Acceleration bx2 = new Acceleration(0.0,
3149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3150 calibrator.getInitialBiasXAsAcceleration(bx2);
3151 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3152 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3153 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3154 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3155 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156 final Acceleration by2 = new Acceleration(0.0,
3157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3158 calibrator.getInitialBiasYAsAcceleration(by2);
3159 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3160 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3161 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3162 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3163 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3164 final Acceleration bz2 = new Acceleration(0.0,
3165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3166 calibrator.getInitialBiasZAsAcceleration(bz2);
3167 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3168 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3169 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3170 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3171 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3172 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3173 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3174 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3175 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3176 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3177 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3178 final double[] bias1 = calibrator.getInitialBias();
3179 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3180 final double[] bias2 = new double[3];
3181 calibrator.getInitialBias(bias2);
3182 assertArrayEquals(bias1, bias2, 0.0);
3183 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3184 assertEquals(b1, ba);
3185 final Matrix b2 = new Matrix(3, 1);
3186 calibrator.getInitialBiasAsMatrix(b2);
3187 assertEquals(b1, b2);
3188 final Matrix ma1 = calibrator.getInitialMa();
3189 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3190 final Matrix ma2 = new Matrix(3, 3);
3191 calibrator.getInitialMa(ma2);
3192 assertEquals(ma1, ma2);
3193 assertSame(calibrator.getMeasurements(), measurements);
3194 assertTrue(calibrator.isCommonAxisUsed());
3195 assertSame(calibrator.getListener(), this);
3196 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3197 assertFalse(calibrator.isReady());
3198 assertFalse(calibrator.isRunning());
3199 assertNull(calibrator.getEstimatedBiases());
3200 assertFalse(calibrator.getEstimatedBiases(null));
3201 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3202 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3203 assertNull(calibrator.getEstimatedBiasFx());
3204 assertNull(calibrator.getEstimatedBiasFy());
3205 assertNull(calibrator.getEstimatedBiasFz());
3206 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3207 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3208 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3209 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3210 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3211 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3212 assertNull(calibrator.getEstimatedMa());
3213 assertNull(calibrator.getEstimatedSx());
3214 assertNull(calibrator.getEstimatedSy());
3215 assertNull(calibrator.getEstimatedSz());
3216 assertNull(calibrator.getEstimatedMxy());
3217 assertNull(calibrator.getEstimatedMxz());
3218 assertNull(calibrator.getEstimatedMyx());
3219 assertNull(calibrator.getEstimatedMyz());
3220 assertNull(calibrator.getEstimatedMzx());
3221 assertNull(calibrator.getEstimatedMzy());
3222 assertNull(calibrator.getEstimatedCovariance());
3223 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3224 assertNull(calibrator.getGroundTruthGravityNorm());
3225 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3226 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3227 }
3228
3229 @Test
3230 public void testConstructor32() throws WrongSizeException {
3231 final Matrix ba = generateBa();
3232 final double biasX = ba.getElementAtIndex(0);
3233 final double biasY = ba.getElementAtIndex(1);
3234 final double biasZ = ba.getElementAtIndex(2);
3235
3236 final Matrix ma = generateMaCommonAxis();
3237 final double sx = ma.getElementAt(0, 0);
3238 final double sy = ma.getElementAt(1, 1);
3239 final double sz = ma.getElementAt(2, 2);
3240
3241 final Acceleration bx = new Acceleration(biasX,
3242 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3243 final Acceleration by = new Acceleration(biasY,
3244 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3245 final Acceleration bz = new Acceleration(biasZ,
3246 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3247
3248 final KnownGravityNormAccelerometerCalibrator calibrator =
3249 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
3250 sx, sy, sz);
3251
3252
3253 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3254 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3255 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3256 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3257 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3258 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3259 final Acceleration bx2 = new Acceleration(0.0,
3260 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3261 calibrator.getInitialBiasXAsAcceleration(bx2);
3262 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3263 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3264 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3265 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3266 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3267 final Acceleration by2 = new Acceleration(0.0,
3268 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3269 calibrator.getInitialBiasYAsAcceleration(by2);
3270 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3271 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3272 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3273 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3274 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3275 final Acceleration bz2 = new Acceleration(0.0,
3276 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3277 calibrator.getInitialBiasZAsAcceleration(bz2);
3278 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3279 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3280 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3281 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3282 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3283 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3284 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3285 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3286 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3287 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3288 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3289 final double[] bias1 = calibrator.getInitialBias();
3290 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3291 final double[] bias2 = new double[3];
3292 calibrator.getInitialBias(bias2);
3293 assertArrayEquals(bias1, bias2, 0.0);
3294 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3295 assertEquals(b1, ba);
3296 final Matrix b2 = new Matrix(3, 1);
3297 calibrator.getInitialBiasAsMatrix(b2);
3298 assertEquals(b1, b2);
3299 final Matrix ma1 = calibrator.getInitialMa();
3300 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3301 final Matrix ma2 = new Matrix(3, 3);
3302 calibrator.getInitialMa(ma2);
3303 assertEquals(ma1, ma2);
3304 assertNull(calibrator.getMeasurements());
3305 assertFalse(calibrator.isCommonAxisUsed());
3306 assertNull(calibrator.getListener());
3307 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3308 assertFalse(calibrator.isReady());
3309 assertFalse(calibrator.isRunning());
3310 assertNull(calibrator.getEstimatedBiases());
3311 assertFalse(calibrator.getEstimatedBiases(null));
3312 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3313 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3314 assertNull(calibrator.getEstimatedBiasFx());
3315 assertNull(calibrator.getEstimatedBiasFy());
3316 assertNull(calibrator.getEstimatedBiasFz());
3317 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3318 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3319 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3320 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3321 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3322 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3323 assertNull(calibrator.getEstimatedMa());
3324 assertNull(calibrator.getEstimatedSx());
3325 assertNull(calibrator.getEstimatedSy());
3326 assertNull(calibrator.getEstimatedSz());
3327 assertNull(calibrator.getEstimatedMxy());
3328 assertNull(calibrator.getEstimatedMxz());
3329 assertNull(calibrator.getEstimatedMyx());
3330 assertNull(calibrator.getEstimatedMyz());
3331 assertNull(calibrator.getEstimatedMzx());
3332 assertNull(calibrator.getEstimatedMzy());
3333 assertNull(calibrator.getEstimatedCovariance());
3334 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3335 assertNull(calibrator.getGroundTruthGravityNorm());
3336 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3337 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3338 }
3339
3340 @Test
3341 public void testConstructor33() throws WrongSizeException {
3342 final Matrix ba = generateBa();
3343 final double biasX = ba.getElementAtIndex(0);
3344 final double biasY = ba.getElementAtIndex(1);
3345 final double biasZ = ba.getElementAtIndex(2);
3346
3347 final Matrix ma = generateMaCommonAxis();
3348 final double sx = ma.getElementAt(0, 0);
3349 final double sy = ma.getElementAt(1, 1);
3350 final double sz = ma.getElementAt(2, 2);
3351
3352 final Acceleration bx = new Acceleration(biasX,
3353 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3354 final Acceleration by = new Acceleration(biasY,
3355 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3356 final Acceleration bz = new Acceleration(biasZ,
3357 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3358
3359 final KnownGravityNormAccelerometerCalibrator calibrator =
3360 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
3361 sx, sy, sz, this);
3362
3363
3364 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3365 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3366 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3367 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3368 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3369 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3370 final Acceleration bx2 = new Acceleration(0.0,
3371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3372 calibrator.getInitialBiasXAsAcceleration(bx2);
3373 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3374 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3375 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3376 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3377 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3378 final Acceleration by2 = new Acceleration(0.0,
3379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3380 calibrator.getInitialBiasYAsAcceleration(by2);
3381 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3382 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3383 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3384 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3385 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3386 final Acceleration bz2 = new Acceleration(0.0,
3387 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3388 calibrator.getInitialBiasZAsAcceleration(bz2);
3389 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3390 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3391 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3392 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3393 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3394 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3395 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3396 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3397 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3398 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3399 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3400 final double[] bias1 = calibrator.getInitialBias();
3401 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3402 final double[] bias2 = new double[3];
3403 calibrator.getInitialBias(bias2);
3404 assertArrayEquals(bias1, bias2, 0.0);
3405 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3406 assertEquals(b1, ba);
3407 final Matrix b2 = new Matrix(3, 1);
3408 calibrator.getInitialBiasAsMatrix(b2);
3409 assertEquals(b1, b2);
3410 final Matrix ma1 = calibrator.getInitialMa();
3411 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3412 final Matrix ma2 = new Matrix(3, 3);
3413 calibrator.getInitialMa(ma2);
3414 assertEquals(ma1, ma2);
3415 assertNull(calibrator.getMeasurements());
3416 assertFalse(calibrator.isCommonAxisUsed());
3417 assertSame(calibrator.getListener(), this);
3418 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3419 assertFalse(calibrator.isReady());
3420 assertFalse(calibrator.isRunning());
3421 assertNull(calibrator.getEstimatedBiases());
3422 assertFalse(calibrator.getEstimatedBiases(null));
3423 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3424 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3425 assertNull(calibrator.getEstimatedBiasFx());
3426 assertNull(calibrator.getEstimatedBiasFy());
3427 assertNull(calibrator.getEstimatedBiasFz());
3428 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3429 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3430 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3431 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3432 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3433 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3434 assertNull(calibrator.getEstimatedMa());
3435 assertNull(calibrator.getEstimatedSx());
3436 assertNull(calibrator.getEstimatedSy());
3437 assertNull(calibrator.getEstimatedSz());
3438 assertNull(calibrator.getEstimatedMxy());
3439 assertNull(calibrator.getEstimatedMxz());
3440 assertNull(calibrator.getEstimatedMyx());
3441 assertNull(calibrator.getEstimatedMyz());
3442 assertNull(calibrator.getEstimatedMzx());
3443 assertNull(calibrator.getEstimatedMzy());
3444 assertNull(calibrator.getEstimatedCovariance());
3445 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3446 assertNull(calibrator.getGroundTruthGravityNorm());
3447 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3448 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3449 }
3450
3451 @Test
3452 public void testConstructor34() throws WrongSizeException {
3453 final Collection<StandardDeviationBodyKinematics> measurements =
3454 Collections.emptyList();
3455
3456 final Matrix ba = generateBa();
3457 final double biasX = ba.getElementAtIndex(0);
3458 final double biasY = ba.getElementAtIndex(1);
3459 final double biasZ = ba.getElementAtIndex(2);
3460
3461 final Matrix ma = generateMaCommonAxis();
3462 final double sx = ma.getElementAt(0, 0);
3463 final double sy = ma.getElementAt(1, 1);
3464 final double sz = ma.getElementAt(2, 2);
3465
3466 final Acceleration bx = new Acceleration(biasX,
3467 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3468 final Acceleration by = new Acceleration(biasY,
3469 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3470 final Acceleration bz = new Acceleration(biasZ,
3471 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3472
3473 final KnownGravityNormAccelerometerCalibrator calibrator =
3474 new KnownGravityNormAccelerometerCalibrator(measurements,
3475 bx, by, bz, sx, sy, sz);
3476
3477
3478 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3479 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3480 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3481 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3482 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3483 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3484 final Acceleration bx2 = new Acceleration(0.0,
3485 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3486 calibrator.getInitialBiasXAsAcceleration(bx2);
3487 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3488 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3489 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3490 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3491 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492 final Acceleration by2 = new Acceleration(0.0,
3493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3494 calibrator.getInitialBiasYAsAcceleration(by2);
3495 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3496 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3497 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3498 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3499 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3500 final Acceleration bz2 = new Acceleration(0.0,
3501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3502 calibrator.getInitialBiasZAsAcceleration(bz2);
3503 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3504 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3505 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3506 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3507 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3508 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3509 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3510 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3511 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3512 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3513 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3514 final double[] bias1 = calibrator.getInitialBias();
3515 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3516 final double[] bias2 = new double[3];
3517 calibrator.getInitialBias(bias2);
3518 assertArrayEquals(bias1, bias2, 0.0);
3519 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3520 assertEquals(b1, ba);
3521 final Matrix b2 = new Matrix(3, 1);
3522 calibrator.getInitialBiasAsMatrix(b2);
3523 assertEquals(b1, b2);
3524 final Matrix ma1 = calibrator.getInitialMa();
3525 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3526 final Matrix ma2 = new Matrix(3, 3);
3527 calibrator.getInitialMa(ma2);
3528 assertEquals(ma1, ma2);
3529 assertSame(calibrator.getMeasurements(), measurements);
3530 assertFalse(calibrator.isCommonAxisUsed());
3531 assertNull(calibrator.getListener());
3532 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3533 assertFalse(calibrator.isReady());
3534 assertFalse(calibrator.isRunning());
3535 assertNull(calibrator.getEstimatedBiases());
3536 assertFalse(calibrator.getEstimatedBiases(null));
3537 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3538 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3539 assertNull(calibrator.getEstimatedBiasFx());
3540 assertNull(calibrator.getEstimatedBiasFy());
3541 assertNull(calibrator.getEstimatedBiasFz());
3542 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3543 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3544 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3545 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3546 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3547 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3548 assertNull(calibrator.getEstimatedMa());
3549 assertNull(calibrator.getEstimatedSx());
3550 assertNull(calibrator.getEstimatedSy());
3551 assertNull(calibrator.getEstimatedSz());
3552 assertNull(calibrator.getEstimatedMxy());
3553 assertNull(calibrator.getEstimatedMxz());
3554 assertNull(calibrator.getEstimatedMyx());
3555 assertNull(calibrator.getEstimatedMyz());
3556 assertNull(calibrator.getEstimatedMzx());
3557 assertNull(calibrator.getEstimatedMzy());
3558 assertNull(calibrator.getEstimatedCovariance());
3559 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3560 assertNull(calibrator.getGroundTruthGravityNorm());
3561 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3562 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3563 }
3564
3565 @Test
3566 public void testConstructor35() throws WrongSizeException {
3567 final Collection<StandardDeviationBodyKinematics> measurements =
3568 Collections.emptyList();
3569
3570 final Matrix ba = generateBa();
3571 final double biasX = ba.getElementAtIndex(0);
3572 final double biasY = ba.getElementAtIndex(1);
3573 final double biasZ = ba.getElementAtIndex(2);
3574
3575 final Matrix ma = generateMaCommonAxis();
3576 final double sx = ma.getElementAt(0, 0);
3577 final double sy = ma.getElementAt(1, 1);
3578 final double sz = ma.getElementAt(2, 2);
3579
3580 final Acceleration bx = new Acceleration(biasX,
3581 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582 final Acceleration by = new Acceleration(biasY,
3583 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3584 final Acceleration bz = new Acceleration(biasZ,
3585 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3586
3587 final KnownGravityNormAccelerometerCalibrator calibrator =
3588 new KnownGravityNormAccelerometerCalibrator(measurements,
3589 bx, by, bz, sx, sy, sz, this);
3590
3591
3592 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3593 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3594 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3595 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3596 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3597 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3598 final Acceleration bx2 = new Acceleration(0.0,
3599 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3600 calibrator.getInitialBiasXAsAcceleration(bx2);
3601 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3602 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3603 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3604 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3605 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3606 final Acceleration by2 = new Acceleration(0.0,
3607 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3608 calibrator.getInitialBiasYAsAcceleration(by2);
3609 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3610 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3611 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3612 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3613 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3614 final Acceleration bz2 = new Acceleration(0.0,
3615 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3616 calibrator.getInitialBiasZAsAcceleration(bz2);
3617 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3618 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3619 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3620 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3621 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3622 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3623 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3624 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3625 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3626 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3627 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3628 final double[] bias1 = calibrator.getInitialBias();
3629 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3630 final double[] bias2 = new double[3];
3631 calibrator.getInitialBias(bias2);
3632 assertArrayEquals(bias1, bias2, 0.0);
3633 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3634 assertEquals(b1, ba);
3635 final Matrix b2 = new Matrix(3, 1);
3636 calibrator.getInitialBiasAsMatrix(b2);
3637 assertEquals(b1, b2);
3638 final Matrix ma1 = calibrator.getInitialMa();
3639 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3640 final Matrix ma2 = new Matrix(3, 3);
3641 calibrator.getInitialMa(ma2);
3642 assertEquals(ma1, ma2);
3643 assertSame(calibrator.getMeasurements(), measurements);
3644 assertFalse(calibrator.isCommonAxisUsed());
3645 assertSame(calibrator.getListener(), this);
3646 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3647 assertFalse(calibrator.isReady());
3648 assertFalse(calibrator.isRunning());
3649 assertNull(calibrator.getEstimatedBiases());
3650 assertFalse(calibrator.getEstimatedBiases(null));
3651 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3652 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3653 assertNull(calibrator.getEstimatedBiasFx());
3654 assertNull(calibrator.getEstimatedBiasFy());
3655 assertNull(calibrator.getEstimatedBiasFz());
3656 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3657 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3658 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3659 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3660 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3661 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3662 assertNull(calibrator.getEstimatedMa());
3663 assertNull(calibrator.getEstimatedSx());
3664 assertNull(calibrator.getEstimatedSy());
3665 assertNull(calibrator.getEstimatedSz());
3666 assertNull(calibrator.getEstimatedMxy());
3667 assertNull(calibrator.getEstimatedMxz());
3668 assertNull(calibrator.getEstimatedMyx());
3669 assertNull(calibrator.getEstimatedMyz());
3670 assertNull(calibrator.getEstimatedMzx());
3671 assertNull(calibrator.getEstimatedMzy());
3672 assertNull(calibrator.getEstimatedCovariance());
3673 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3674 assertNull(calibrator.getGroundTruthGravityNorm());
3675 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3676 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3677 }
3678
3679 @Test
3680 public void testConstructor36() throws WrongSizeException {
3681 final Matrix ba = generateBa();
3682 final double biasX = ba.getElementAtIndex(0);
3683 final double biasY = ba.getElementAtIndex(1);
3684 final double biasZ = ba.getElementAtIndex(2);
3685
3686 final Matrix ma = generateMaCommonAxis();
3687 final double sx = ma.getElementAt(0, 0);
3688 final double sy = ma.getElementAt(1, 1);
3689 final double sz = ma.getElementAt(2, 2);
3690
3691 final Acceleration bx = new Acceleration(biasX,
3692 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3693 final Acceleration by = new Acceleration(biasY,
3694 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3695 final Acceleration bz = new Acceleration(biasZ,
3696 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3697
3698 final KnownGravityNormAccelerometerCalibrator calibrator =
3699 new KnownGravityNormAccelerometerCalibrator(true,
3700 bx, by, bz, sx, sy, sz);
3701
3702
3703 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3704 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3705 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3706 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3707 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3708 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709 final Acceleration bx2 = new Acceleration(0.0,
3710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3711 calibrator.getInitialBiasXAsAcceleration(bx2);
3712 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3713 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3714 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3715 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3716 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717 final Acceleration by2 = new Acceleration(0.0,
3718 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3719 calibrator.getInitialBiasYAsAcceleration(by2);
3720 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3721 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3722 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3723 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3724 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3725 final Acceleration bz2 = new Acceleration(0.0,
3726 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3727 calibrator.getInitialBiasZAsAcceleration(bz2);
3728 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3729 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3730 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3731 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3732 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3733 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3734 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3735 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3736 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3737 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3738 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3739 final double[] bias1 = calibrator.getInitialBias();
3740 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3741 final double[] bias2 = new double[3];
3742 calibrator.getInitialBias(bias2);
3743 assertArrayEquals(bias1, bias2, 0.0);
3744 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3745 assertEquals(b1, ba);
3746 final Matrix b2 = new Matrix(3, 1);
3747 calibrator.getInitialBiasAsMatrix(b2);
3748 assertEquals(b1, b2);
3749 final Matrix ma1 = calibrator.getInitialMa();
3750 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3751 final Matrix ma2 = new Matrix(3, 3);
3752 calibrator.getInitialMa(ma2);
3753 assertEquals(ma1, ma2);
3754 assertNull(calibrator.getMeasurements());
3755 assertTrue(calibrator.isCommonAxisUsed());
3756 assertNull(calibrator.getListener());
3757 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3758 assertFalse(calibrator.isReady());
3759 assertFalse(calibrator.isRunning());
3760 assertNull(calibrator.getEstimatedBiases());
3761 assertFalse(calibrator.getEstimatedBiases(null));
3762 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3763 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3764 assertNull(calibrator.getEstimatedBiasFx());
3765 assertNull(calibrator.getEstimatedBiasFy());
3766 assertNull(calibrator.getEstimatedBiasFz());
3767 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3768 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3769 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3770 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3771 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3772 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3773 assertNull(calibrator.getEstimatedMa());
3774 assertNull(calibrator.getEstimatedSx());
3775 assertNull(calibrator.getEstimatedSy());
3776 assertNull(calibrator.getEstimatedSz());
3777 assertNull(calibrator.getEstimatedMxy());
3778 assertNull(calibrator.getEstimatedMxz());
3779 assertNull(calibrator.getEstimatedMyx());
3780 assertNull(calibrator.getEstimatedMyz());
3781 assertNull(calibrator.getEstimatedMzx());
3782 assertNull(calibrator.getEstimatedMzy());
3783 assertNull(calibrator.getEstimatedCovariance());
3784 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3785 assertNull(calibrator.getGroundTruthGravityNorm());
3786 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3787 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3788 }
3789
3790 @Test
3791 public void testConstructor37() throws WrongSizeException {
3792 final Matrix ba = generateBa();
3793 final double biasX = ba.getElementAtIndex(0);
3794 final double biasY = ba.getElementAtIndex(1);
3795 final double biasZ = ba.getElementAtIndex(2);
3796
3797 final Matrix ma = generateMaCommonAxis();
3798 final double sx = ma.getElementAt(0, 0);
3799 final double sy = ma.getElementAt(1, 1);
3800 final double sz = ma.getElementAt(2, 2);
3801
3802 final Acceleration bx = new Acceleration(biasX,
3803 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3804 final Acceleration by = new Acceleration(biasY,
3805 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3806 final Acceleration bz = new Acceleration(biasZ,
3807 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3808
3809 final KnownGravityNormAccelerometerCalibrator calibrator =
3810 new KnownGravityNormAccelerometerCalibrator(true,
3811 bx, by, bz, sx, sy, sz, this);
3812
3813
3814 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3815 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3816 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3817 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3818 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3819 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3820 final Acceleration bx2 = new Acceleration(0.0,
3821 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3822 calibrator.getInitialBiasXAsAcceleration(bx2);
3823 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3824 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3826 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3827 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3828 final Acceleration by2 = new Acceleration(0.0,
3829 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3830 calibrator.getInitialBiasYAsAcceleration(by2);
3831 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3832 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3834 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3835 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3836 final Acceleration bz2 = new Acceleration(0.0,
3837 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3838 calibrator.getInitialBiasZAsAcceleration(bz2);
3839 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3840 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3841 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3842 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3843 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3844 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3845 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3846 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3847 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3848 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3849 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3850 final double[] bias1 = calibrator.getInitialBias();
3851 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3852 final double[] bias2 = new double[3];
3853 calibrator.getInitialBias(bias2);
3854 assertArrayEquals(bias1, bias2, 0.0);
3855 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3856 assertEquals(b1, ba);
3857 final Matrix b2 = new Matrix(3, 1);
3858 calibrator.getInitialBiasAsMatrix(b2);
3859 assertEquals(b1, b2);
3860 final Matrix ma1 = calibrator.getInitialMa();
3861 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3862 final Matrix ma2 = new Matrix(3, 3);
3863 calibrator.getInitialMa(ma2);
3864 assertEquals(ma1, ma2);
3865 assertNull(calibrator.getMeasurements());
3866 assertTrue(calibrator.isCommonAxisUsed());
3867 assertSame(calibrator.getListener(), this);
3868 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3869 assertFalse(calibrator.isReady());
3870 assertFalse(calibrator.isRunning());
3871 assertNull(calibrator.getEstimatedBiases());
3872 assertFalse(calibrator.getEstimatedBiases(null));
3873 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3874 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3875 assertNull(calibrator.getEstimatedBiasFx());
3876 assertNull(calibrator.getEstimatedBiasFy());
3877 assertNull(calibrator.getEstimatedBiasFz());
3878 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3879 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3880 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3881 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3882 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3883 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3884 assertNull(calibrator.getEstimatedMa());
3885 assertNull(calibrator.getEstimatedSx());
3886 assertNull(calibrator.getEstimatedSy());
3887 assertNull(calibrator.getEstimatedSz());
3888 assertNull(calibrator.getEstimatedMxy());
3889 assertNull(calibrator.getEstimatedMxz());
3890 assertNull(calibrator.getEstimatedMyx());
3891 assertNull(calibrator.getEstimatedMyz());
3892 assertNull(calibrator.getEstimatedMzx());
3893 assertNull(calibrator.getEstimatedMzy());
3894 assertNull(calibrator.getEstimatedCovariance());
3895 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3896 assertNull(calibrator.getGroundTruthGravityNorm());
3897 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3898 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3899 }
3900
3901 @Test
3902 public void testConstructor38() throws WrongSizeException {
3903 final Collection<StandardDeviationBodyKinematics> measurements =
3904 Collections.emptyList();
3905
3906 final Matrix ba = generateBa();
3907 final double biasX = ba.getElementAtIndex(0);
3908 final double biasY = ba.getElementAtIndex(1);
3909 final double biasZ = ba.getElementAtIndex(2);
3910
3911 final Matrix ma = generateMaCommonAxis();
3912 final double sx = ma.getElementAt(0, 0);
3913 final double sy = ma.getElementAt(1, 1);
3914 final double sz = ma.getElementAt(2, 2);
3915
3916 final Acceleration bx = new Acceleration(biasX,
3917 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3918 final Acceleration by = new Acceleration(biasY,
3919 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3920 final Acceleration bz = new Acceleration(biasZ,
3921 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3922
3923 final KnownGravityNormAccelerometerCalibrator calibrator =
3924 new KnownGravityNormAccelerometerCalibrator(measurements,
3925 true, bx, by, bz, sx, sy, sz);
3926
3927
3928 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3929 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3930 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3931 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3932 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3933 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3934 final Acceleration bx2 = new Acceleration(0.0,
3935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3936 calibrator.getInitialBiasXAsAcceleration(bx2);
3937 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3938 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3940 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3941 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3942 final Acceleration by2 = new Acceleration(0.0,
3943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3944 calibrator.getInitialBiasYAsAcceleration(by2);
3945 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3946 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3948 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3949 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3950 final Acceleration bz2 = new Acceleration(0.0,
3951 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3952 calibrator.getInitialBiasZAsAcceleration(bz2);
3953 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3954 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3955 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3956 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3957 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3958 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3959 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3960 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3961 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3962 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3963 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3964 final double[] bias1 = calibrator.getInitialBias();
3965 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3966 final double[] bias2 = new double[3];
3967 calibrator.getInitialBias(bias2);
3968 assertArrayEquals(bias1, bias2, 0.0);
3969 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3970 assertEquals(b1, ba);
3971 final Matrix b2 = new Matrix(3, 1);
3972 calibrator.getInitialBiasAsMatrix(b2);
3973 assertEquals(b1, b2);
3974 final Matrix ma1 = calibrator.getInitialMa();
3975 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3976 final Matrix ma2 = new Matrix(3, 3);
3977 calibrator.getInitialMa(ma2);
3978 assertEquals(ma1, ma2);
3979 assertSame(calibrator.getMeasurements(), measurements);
3980 assertTrue(calibrator.isCommonAxisUsed());
3981 assertNull(calibrator.getListener());
3982 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3983 assertFalse(calibrator.isReady());
3984 assertFalse(calibrator.isRunning());
3985 assertNull(calibrator.getEstimatedBiases());
3986 assertFalse(calibrator.getEstimatedBiases(null));
3987 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3988 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3989 assertNull(calibrator.getEstimatedBiasFx());
3990 assertNull(calibrator.getEstimatedBiasFy());
3991 assertNull(calibrator.getEstimatedBiasFz());
3992 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3993 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3994 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3995 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3996 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3997 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3998 assertNull(calibrator.getEstimatedMa());
3999 assertNull(calibrator.getEstimatedSx());
4000 assertNull(calibrator.getEstimatedSy());
4001 assertNull(calibrator.getEstimatedSz());
4002 assertNull(calibrator.getEstimatedMxy());
4003 assertNull(calibrator.getEstimatedMxz());
4004 assertNull(calibrator.getEstimatedMyx());
4005 assertNull(calibrator.getEstimatedMyz());
4006 assertNull(calibrator.getEstimatedMzx());
4007 assertNull(calibrator.getEstimatedMzy());
4008 assertNull(calibrator.getEstimatedCovariance());
4009 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4010 assertNull(calibrator.getGroundTruthGravityNorm());
4011 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4012 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4013 }
4014
4015 @Test
4016 public void testConstructor39() throws WrongSizeException {
4017 final Collection<StandardDeviationBodyKinematics> measurements =
4018 Collections.emptyList();
4019
4020 final Matrix ba = generateBa();
4021 final double biasX = ba.getElementAtIndex(0);
4022 final double biasY = ba.getElementAtIndex(1);
4023 final double biasZ = ba.getElementAtIndex(2);
4024
4025 final Matrix ma = generateMaCommonAxis();
4026 final double sx = ma.getElementAt(0, 0);
4027 final double sy = ma.getElementAt(1, 1);
4028 final double sz = ma.getElementAt(2, 2);
4029
4030 final Acceleration bx = new Acceleration(biasX,
4031 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4032 final Acceleration by = new Acceleration(biasY,
4033 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4034 final Acceleration bz = new Acceleration(biasZ,
4035 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4036
4037 final KnownGravityNormAccelerometerCalibrator calibrator =
4038 new KnownGravityNormAccelerometerCalibrator(measurements,
4039 true, bx, by, bz, sx, sy, sz,
4040 this);
4041
4042
4043 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4044 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4045 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4046 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4047 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4048 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049 final Acceleration bx2 = new Acceleration(0.0,
4050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051 calibrator.getInitialBiasXAsAcceleration(bx2);
4052 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4053 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4055 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4056 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057 final Acceleration by2 = new Acceleration(0.0,
4058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059 calibrator.getInitialBiasYAsAcceleration(by2);
4060 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4061 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4063 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4064 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4065 final Acceleration bz2 = new Acceleration(0.0,
4066 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4067 calibrator.getInitialBiasZAsAcceleration(bz2);
4068 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4069 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4070 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4071 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4072 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4073 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4074 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4075 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4076 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4077 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4078 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4079 final double[] bias1 = calibrator.getInitialBias();
4080 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4081 final double[] bias2 = new double[3];
4082 calibrator.getInitialBias(bias2);
4083 assertArrayEquals(bias1, bias2, 0.0);
4084 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4085 assertEquals(b1, ba);
4086 final Matrix b2 = new Matrix(3, 1);
4087 calibrator.getInitialBiasAsMatrix(b2);
4088 assertEquals(b1, b2);
4089 final Matrix ma1 = calibrator.getInitialMa();
4090 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4091 final Matrix ma2 = new Matrix(3, 3);
4092 calibrator.getInitialMa(ma2);
4093 assertEquals(ma1, ma2);
4094 assertSame(calibrator.getMeasurements(), measurements);
4095 assertTrue(calibrator.isCommonAxisUsed());
4096 assertSame(calibrator.getListener(), this);
4097 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4098 assertFalse(calibrator.isReady());
4099 assertFalse(calibrator.isRunning());
4100 assertNull(calibrator.getEstimatedBiases());
4101 assertFalse(calibrator.getEstimatedBiases(null));
4102 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4103 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4104 assertNull(calibrator.getEstimatedBiasFx());
4105 assertNull(calibrator.getEstimatedBiasFy());
4106 assertNull(calibrator.getEstimatedBiasFz());
4107 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4108 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4109 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4110 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4111 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4112 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4113 assertNull(calibrator.getEstimatedMa());
4114 assertNull(calibrator.getEstimatedSx());
4115 assertNull(calibrator.getEstimatedSy());
4116 assertNull(calibrator.getEstimatedSz());
4117 assertNull(calibrator.getEstimatedMxy());
4118 assertNull(calibrator.getEstimatedMxz());
4119 assertNull(calibrator.getEstimatedMyx());
4120 assertNull(calibrator.getEstimatedMyz());
4121 assertNull(calibrator.getEstimatedMzx());
4122 assertNull(calibrator.getEstimatedMzy());
4123 assertNull(calibrator.getEstimatedCovariance());
4124 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4125 assertNull(calibrator.getGroundTruthGravityNorm());
4126 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4127 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4128 }
4129
4130 @Test
4131 public void testConstructor40() throws WrongSizeException {
4132 final Matrix ba = generateBa();
4133 final double biasX = ba.getElementAtIndex(0);
4134 final double biasY = ba.getElementAtIndex(1);
4135 final double biasZ = ba.getElementAtIndex(2);
4136
4137 final Matrix ma = generateMaCommonAxis();
4138 final double sx = ma.getElementAt(0, 0);
4139 final double sy = ma.getElementAt(1, 1);
4140 final double sz = ma.getElementAt(2, 2);
4141 final double mxy = ma.getElementAt(0, 1);
4142 final double mxz = ma.getElementAt(0, 2);
4143 final double myx = ma.getElementAt(1, 0);
4144 final double myz = ma.getElementAt(1, 2);
4145 final double mzx = ma.getElementAt(2, 0);
4146 final double mzy = ma.getElementAt(2, 1);
4147
4148 final KnownGravityNormAccelerometerCalibrator calibrator =
4149 new KnownGravityNormAccelerometerCalibrator(
4150 biasX, biasY, biasZ, sx, sy, sz,
4151 mxy, mxz, myx, myz, mzx, mzy);
4152
4153
4154 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4155 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4156 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4157 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4158 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4159 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4160 final Acceleration bx2 = new Acceleration(0.0,
4161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4162 calibrator.getInitialBiasXAsAcceleration(bx2);
4163 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4164 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4165 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4166 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4167 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4168 final Acceleration by2 = new Acceleration(0.0,
4169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4170 calibrator.getInitialBiasYAsAcceleration(by2);
4171 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4172 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4173 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4174 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4175 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4176 final Acceleration bz2 = new Acceleration(0.0,
4177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4178 calibrator.getInitialBiasZAsAcceleration(bz2);
4179 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4180 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4181 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4182 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4183 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4184 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4185 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4186 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4187 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4188 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4189 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4190 final double[] bias1 = calibrator.getInitialBias();
4191 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4192 final double[] bias2 = new double[3];
4193 calibrator.getInitialBias(bias2);
4194 assertArrayEquals(bias1, bias2, 0.0);
4195 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4196 assertEquals(b1, ba);
4197 final Matrix b2 = new Matrix(3, 1);
4198 calibrator.getInitialBiasAsMatrix(b2);
4199 assertEquals(b1, b2);
4200 final Matrix ma1 = new Matrix(3, 3);
4201 ma1.setSubmatrix(0, 0,
4202 2, 2,
4203 new double[]{sx, myx, mzx,
4204 mxy, sy, mzy,
4205 mxz, myz, sz});
4206 assertEquals(calibrator.getInitialMa(), ma1);
4207 final Matrix ma2 = new Matrix(3, 3);
4208 calibrator.getInitialMa(ma2);
4209 assertEquals(ma1, ma2);
4210 assertNull(calibrator.getMeasurements());
4211 assertFalse(calibrator.isCommonAxisUsed());
4212 assertNull(calibrator.getListener());
4213 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4214 assertFalse(calibrator.isReady());
4215 assertFalse(calibrator.isRunning());
4216 assertNull(calibrator.getEstimatedBiases());
4217 assertFalse(calibrator.getEstimatedBiases(null));
4218 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4219 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4220 assertNull(calibrator.getEstimatedBiasFx());
4221 assertNull(calibrator.getEstimatedBiasFy());
4222 assertNull(calibrator.getEstimatedBiasFz());
4223 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4224 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4225 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4226 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4227 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4228 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4229 assertNull(calibrator.getEstimatedMa());
4230 assertNull(calibrator.getEstimatedSx());
4231 assertNull(calibrator.getEstimatedSy());
4232 assertNull(calibrator.getEstimatedSz());
4233 assertNull(calibrator.getEstimatedMxy());
4234 assertNull(calibrator.getEstimatedMxz());
4235 assertNull(calibrator.getEstimatedMyx());
4236 assertNull(calibrator.getEstimatedMyz());
4237 assertNull(calibrator.getEstimatedMzx());
4238 assertNull(calibrator.getEstimatedMzy());
4239 assertNull(calibrator.getEstimatedCovariance());
4240 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4241 assertNull(calibrator.getGroundTruthGravityNorm());
4242 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4243 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4244 }
4245
4246 @Test
4247 public void testConstructor41() throws WrongSizeException {
4248 final Collection<StandardDeviationBodyKinematics> measurements =
4249 Collections.emptyList();
4250
4251 final Matrix ba = generateBa();
4252 final double biasX = ba.getElementAtIndex(0);
4253 final double biasY = ba.getElementAtIndex(1);
4254 final double biasZ = ba.getElementAtIndex(2);
4255
4256 final Matrix ma = generateMaCommonAxis();
4257 final double sx = ma.getElementAt(0, 0);
4258 final double sy = ma.getElementAt(1, 1);
4259 final double sz = ma.getElementAt(2, 2);
4260 final double mxy = ma.getElementAt(0, 1);
4261 final double mxz = ma.getElementAt(0, 2);
4262 final double myx = ma.getElementAt(1, 0);
4263 final double myz = ma.getElementAt(1, 2);
4264 final double mzx = ma.getElementAt(2, 0);
4265 final double mzy = ma.getElementAt(2, 1);
4266
4267 final KnownGravityNormAccelerometerCalibrator calibrator =
4268 new KnownGravityNormAccelerometerCalibrator(measurements,
4269 biasX, biasY, biasZ, sx, sy, sz,
4270 mxy, mxz, myx, myz, mzx, mzy);
4271
4272
4273 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4274 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4275 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4276 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4277 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4278 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4279 final Acceleration bx2 = new Acceleration(0.0,
4280 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4281 calibrator.getInitialBiasXAsAcceleration(bx2);
4282 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4283 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4284 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4285 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4286 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4287 final Acceleration by2 = new Acceleration(0.0,
4288 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4289 calibrator.getInitialBiasYAsAcceleration(by2);
4290 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4291 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4292 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4293 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4294 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4295 final Acceleration bz2 = new Acceleration(0.0,
4296 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4297 calibrator.getInitialBiasZAsAcceleration(bz2);
4298 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4299 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4300 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4301 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4302 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4303 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4304 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4305 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4306 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4307 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4308 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4309 final double[] bias1 = calibrator.getInitialBias();
4310 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4311 final double[] bias2 = new double[3];
4312 calibrator.getInitialBias(bias2);
4313 assertArrayEquals(bias1, bias2, 0.0);
4314 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4315 assertEquals(b1, ba);
4316 final Matrix b2 = new Matrix(3, 1);
4317 calibrator.getInitialBiasAsMatrix(b2);
4318 assertEquals(b1, b2);
4319 final Matrix ma1 = new Matrix(3, 3);
4320 ma1.setSubmatrix(0, 0,
4321 2, 2,
4322 new double[]{sx, myx, mzx,
4323 mxy, sy, mzy,
4324 mxz, myz, sz});
4325 assertEquals(calibrator.getInitialMa(), ma1);
4326 final Matrix ma2 = new Matrix(3, 3);
4327 calibrator.getInitialMa(ma2);
4328 assertEquals(ma1, ma2);
4329 assertSame(calibrator.getMeasurements(), measurements);
4330 assertFalse(calibrator.isCommonAxisUsed());
4331 assertNull(calibrator.getListener());
4332 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4333 assertFalse(calibrator.isReady());
4334 assertFalse(calibrator.isRunning());
4335 assertNull(calibrator.getEstimatedBiases());
4336 assertFalse(calibrator.getEstimatedBiases(null));
4337 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4338 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4339 assertNull(calibrator.getEstimatedBiasFx());
4340 assertNull(calibrator.getEstimatedBiasFy());
4341 assertNull(calibrator.getEstimatedBiasFz());
4342 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4343 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4344 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4345 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4346 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4347 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4348 assertNull(calibrator.getEstimatedMa());
4349 assertNull(calibrator.getEstimatedSx());
4350 assertNull(calibrator.getEstimatedSy());
4351 assertNull(calibrator.getEstimatedSz());
4352 assertNull(calibrator.getEstimatedMxy());
4353 assertNull(calibrator.getEstimatedMxz());
4354 assertNull(calibrator.getEstimatedMyx());
4355 assertNull(calibrator.getEstimatedMyz());
4356 assertNull(calibrator.getEstimatedMzx());
4357 assertNull(calibrator.getEstimatedMzy());
4358 assertNull(calibrator.getEstimatedCovariance());
4359 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4360 assertNull(calibrator.getGroundTruthGravityNorm());
4361 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4362 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4363 }
4364
4365 @Test
4366 public void testConstructor42() throws WrongSizeException {
4367 final Collection<StandardDeviationBodyKinematics> measurements =
4368 Collections.emptyList();
4369
4370 final Matrix ba = generateBa();
4371 final double biasX = ba.getElementAtIndex(0);
4372 final double biasY = ba.getElementAtIndex(1);
4373 final double biasZ = ba.getElementAtIndex(2);
4374
4375 final Matrix ma = generateMaCommonAxis();
4376 final double sx = ma.getElementAt(0, 0);
4377 final double sy = ma.getElementAt(1, 1);
4378 final double sz = ma.getElementAt(2, 2);
4379 final double mxy = ma.getElementAt(0, 1);
4380 final double mxz = ma.getElementAt(0, 2);
4381 final double myx = ma.getElementAt(1, 0);
4382 final double myz = ma.getElementAt(1, 2);
4383 final double mzx = ma.getElementAt(2, 0);
4384 final double mzy = ma.getElementAt(2, 1);
4385
4386 final KnownGravityNormAccelerometerCalibrator calibrator =
4387 new KnownGravityNormAccelerometerCalibrator(measurements,
4388 biasX, biasY, biasZ, sx, sy, sz,
4389 mxy, mxz, myx, myz, mzx, mzy, this);
4390
4391
4392 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4393 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4394 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4395 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4396 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4397 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4398 final Acceleration bx2 = new Acceleration(0.0,
4399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4400 calibrator.getInitialBiasXAsAcceleration(bx2);
4401 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4402 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4403 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4404 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4405 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4406 final Acceleration by2 = new Acceleration(0.0,
4407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4408 calibrator.getInitialBiasYAsAcceleration(by2);
4409 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4410 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4411 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4412 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4413 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414 final Acceleration bz2 = new Acceleration(0.0,
4415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4416 calibrator.getInitialBiasZAsAcceleration(bz2);
4417 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4418 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4419 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4420 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4421 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4422 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4423 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4424 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4425 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4426 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4427 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4428 final double[] bias1 = calibrator.getInitialBias();
4429 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4430 final double[] bias2 = new double[3];
4431 calibrator.getInitialBias(bias2);
4432 assertArrayEquals(bias1, bias2, 0.0);
4433 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4434 assertEquals(b1, ba);
4435 final Matrix b2 = new Matrix(3, 1);
4436 calibrator.getInitialBiasAsMatrix(b2);
4437 assertEquals(b1, b2);
4438 final Matrix ma1 = new Matrix(3, 3);
4439 ma1.setSubmatrix(0, 0,
4440 2, 2,
4441 new double[]{sx, myx, mzx,
4442 mxy, sy, mzy,
4443 mxz, myz, sz});
4444 assertEquals(calibrator.getInitialMa(), ma1);
4445 final Matrix ma2 = new Matrix(3, 3);
4446 calibrator.getInitialMa(ma2);
4447 assertEquals(ma1, ma2);
4448 assertSame(calibrator.getMeasurements(), measurements);
4449 assertFalse(calibrator.isCommonAxisUsed());
4450 assertSame(calibrator.getListener(), this);
4451 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4452 assertFalse(calibrator.isReady());
4453 assertFalse(calibrator.isRunning());
4454 assertNull(calibrator.getEstimatedBiases());
4455 assertFalse(calibrator.getEstimatedBiases(null));
4456 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4457 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4458 assertNull(calibrator.getEstimatedBiasFx());
4459 assertNull(calibrator.getEstimatedBiasFy());
4460 assertNull(calibrator.getEstimatedBiasFz());
4461 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4462 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4463 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4464 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4465 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4466 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4467 assertNull(calibrator.getEstimatedMa());
4468 assertNull(calibrator.getEstimatedSx());
4469 assertNull(calibrator.getEstimatedSy());
4470 assertNull(calibrator.getEstimatedSz());
4471 assertNull(calibrator.getEstimatedMxy());
4472 assertNull(calibrator.getEstimatedMxz());
4473 assertNull(calibrator.getEstimatedMyx());
4474 assertNull(calibrator.getEstimatedMyz());
4475 assertNull(calibrator.getEstimatedMzx());
4476 assertNull(calibrator.getEstimatedMzy());
4477 assertNull(calibrator.getEstimatedCovariance());
4478 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4479 assertNull(calibrator.getGroundTruthGravityNorm());
4480 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4481 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4482 }
4483
4484 @Test
4485 public void testConstructor43() throws WrongSizeException {
4486 final Matrix ba = generateBa();
4487 final double biasX = ba.getElementAtIndex(0);
4488 final double biasY = ba.getElementAtIndex(1);
4489 final double biasZ = ba.getElementAtIndex(2);
4490
4491 final Matrix ma = generateMaCommonAxis();
4492 final double sx = ma.getElementAt(0, 0);
4493 final double sy = ma.getElementAt(1, 1);
4494 final double sz = ma.getElementAt(2, 2);
4495 final double mxy = ma.getElementAt(0, 1);
4496 final double mxz = ma.getElementAt(0, 2);
4497 final double myx = ma.getElementAt(1, 0);
4498 final double myz = ma.getElementAt(1, 2);
4499 final double mzx = ma.getElementAt(2, 0);
4500 final double mzy = ma.getElementAt(2, 1);
4501
4502 final KnownGravityNormAccelerometerCalibrator calibrator =
4503 new KnownGravityNormAccelerometerCalibrator(
4504 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4505 myz, mzx, mzy);
4506
4507
4508 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4509 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4510 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4511 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4512 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4513 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4514 final Acceleration bx2 = new Acceleration(0.0,
4515 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4516 calibrator.getInitialBiasXAsAcceleration(bx2);
4517 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4518 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4519 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4520 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4521 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4522 final Acceleration by2 = new Acceleration(0.0,
4523 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4524 calibrator.getInitialBiasYAsAcceleration(by2);
4525 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4526 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4527 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4528 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4529 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4530 final Acceleration bz2 = new Acceleration(0.0,
4531 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4532 calibrator.getInitialBiasZAsAcceleration(bz2);
4533 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4534 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4535 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4536 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4537 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4538 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4539 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4540 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4541 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4542 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4543 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4544 final double[] bias1 = calibrator.getInitialBias();
4545 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4546 final double[] bias2 = new double[3];
4547 calibrator.getInitialBias(bias2);
4548 assertArrayEquals(bias1, bias2, 0.0);
4549 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4550 assertEquals(b1, ba);
4551 final Matrix b2 = new Matrix(3, 1);
4552 calibrator.getInitialBiasAsMatrix(b2);
4553 assertEquals(b1, b2);
4554 final Matrix ma1 = new Matrix(3, 3);
4555 ma1.setSubmatrix(0, 0,
4556 2, 2,
4557 new double[]{sx, myx, mzx,
4558 mxy, sy, mzy,
4559 mxz, myz, sz});
4560 assertEquals(calibrator.getInitialMa(), ma1);
4561 final Matrix ma2 = new Matrix(3, 3);
4562 calibrator.getInitialMa(ma2);
4563 assertEquals(ma1, ma2);
4564 assertNull(calibrator.getMeasurements());
4565 assertTrue(calibrator.isCommonAxisUsed());
4566 assertNull(calibrator.getListener());
4567 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4568 assertFalse(calibrator.isReady());
4569 assertFalse(calibrator.isRunning());
4570 assertNull(calibrator.getEstimatedBiases());
4571 assertFalse(calibrator.getEstimatedBiases(null));
4572 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4573 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4574 assertNull(calibrator.getEstimatedBiasFx());
4575 assertNull(calibrator.getEstimatedBiasFy());
4576 assertNull(calibrator.getEstimatedBiasFz());
4577 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4578 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4579 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4580 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4581 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4582 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4583 assertNull(calibrator.getEstimatedMa());
4584 assertNull(calibrator.getEstimatedSx());
4585 assertNull(calibrator.getEstimatedSy());
4586 assertNull(calibrator.getEstimatedSz());
4587 assertNull(calibrator.getEstimatedMxy());
4588 assertNull(calibrator.getEstimatedMxz());
4589 assertNull(calibrator.getEstimatedMyx());
4590 assertNull(calibrator.getEstimatedMyz());
4591 assertNull(calibrator.getEstimatedMzx());
4592 assertNull(calibrator.getEstimatedMzy());
4593 assertNull(calibrator.getEstimatedCovariance());
4594 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4595 assertNull(calibrator.getGroundTruthGravityNorm());
4596 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4597 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4598 }
4599
4600 @Test
4601 public void testConstructor44() throws WrongSizeException {
4602 final Matrix ba = generateBa();
4603 final double biasX = ba.getElementAtIndex(0);
4604 final double biasY = ba.getElementAtIndex(1);
4605 final double biasZ = ba.getElementAtIndex(2);
4606
4607 final Matrix ma = generateMaCommonAxis();
4608 final double sx = ma.getElementAt(0, 0);
4609 final double sy = ma.getElementAt(1, 1);
4610 final double sz = ma.getElementAt(2, 2);
4611 final double mxy = ma.getElementAt(0, 1);
4612 final double mxz = ma.getElementAt(0, 2);
4613 final double myx = ma.getElementAt(1, 0);
4614 final double myz = ma.getElementAt(1, 2);
4615 final double mzx = ma.getElementAt(2, 0);
4616 final double mzy = ma.getElementAt(2, 1);
4617
4618 final KnownGravityNormAccelerometerCalibrator calibrator =
4619 new KnownGravityNormAccelerometerCalibrator(
4620 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4621 myz, mzx, mzy, this);
4622
4623
4624 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4625 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4626 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4627 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4628 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4629 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4630 final Acceleration bx2 = new Acceleration(0.0,
4631 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4632 calibrator.getInitialBiasXAsAcceleration(bx2);
4633 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4634 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4635 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4636 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4637 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4638 final Acceleration by2 = new Acceleration(0.0,
4639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4640 calibrator.getInitialBiasYAsAcceleration(by2);
4641 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4642 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4643 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4644 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4645 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4646 final Acceleration bz2 = new Acceleration(0.0,
4647 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4648 calibrator.getInitialBiasZAsAcceleration(bz2);
4649 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4650 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4651 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4652 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4653 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4654 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4655 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4656 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4657 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4658 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4659 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4660 final double[] bias1 = calibrator.getInitialBias();
4661 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4662 final double[] bias2 = new double[3];
4663 calibrator.getInitialBias(bias2);
4664 assertArrayEquals(bias1, bias2, 0.0);
4665 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4666 assertEquals(b1, ba);
4667 final Matrix b2 = new Matrix(3, 1);
4668 calibrator.getInitialBiasAsMatrix(b2);
4669 assertEquals(b1, b2);
4670 final Matrix ma1 = new Matrix(3, 3);
4671 ma1.setSubmatrix(0, 0,
4672 2, 2,
4673 new double[]{sx, myx, mzx,
4674 mxy, sy, mzy,
4675 mxz, myz, sz});
4676 assertEquals(calibrator.getInitialMa(), ma1);
4677 final Matrix ma2 = new Matrix(3, 3);
4678 calibrator.getInitialMa(ma2);
4679 assertEquals(ma1, ma2);
4680 assertNull(calibrator.getMeasurements());
4681 assertTrue(calibrator.isCommonAxisUsed());
4682 assertSame(calibrator.getListener(), this);
4683 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4684 assertFalse(calibrator.isReady());
4685 assertFalse(calibrator.isRunning());
4686 assertNull(calibrator.getEstimatedBiases());
4687 assertFalse(calibrator.getEstimatedBiases(null));
4688 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4689 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4690 assertNull(calibrator.getEstimatedBiasFx());
4691 assertNull(calibrator.getEstimatedBiasFy());
4692 assertNull(calibrator.getEstimatedBiasFz());
4693 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4694 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4695 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4696 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4697 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4698 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4699 assertNull(calibrator.getEstimatedMa());
4700 assertNull(calibrator.getEstimatedSx());
4701 assertNull(calibrator.getEstimatedSy());
4702 assertNull(calibrator.getEstimatedSz());
4703 assertNull(calibrator.getEstimatedMxy());
4704 assertNull(calibrator.getEstimatedMxz());
4705 assertNull(calibrator.getEstimatedMyx());
4706 assertNull(calibrator.getEstimatedMyz());
4707 assertNull(calibrator.getEstimatedMzx());
4708 assertNull(calibrator.getEstimatedMzy());
4709 assertNull(calibrator.getEstimatedCovariance());
4710 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4711 assertNull(calibrator.getGroundTruthGravityNorm());
4712 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4713 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4714 }
4715
4716 @Test
4717 public void testConstructor45() throws WrongSizeException {
4718 final Collection<StandardDeviationBodyKinematics> measurements =
4719 Collections.emptyList();
4720
4721 final Matrix ba = generateBa();
4722 final double biasX = ba.getElementAtIndex(0);
4723 final double biasY = ba.getElementAtIndex(1);
4724 final double biasZ = ba.getElementAtIndex(2);
4725
4726 final Matrix ma = generateMaCommonAxis();
4727 final double sx = ma.getElementAt(0, 0);
4728 final double sy = ma.getElementAt(1, 1);
4729 final double sz = ma.getElementAt(2, 2);
4730 final double mxy = ma.getElementAt(0, 1);
4731 final double mxz = ma.getElementAt(0, 2);
4732 final double myx = ma.getElementAt(1, 0);
4733 final double myz = ma.getElementAt(1, 2);
4734 final double mzx = ma.getElementAt(2, 0);
4735 final double mzy = ma.getElementAt(2, 1);
4736
4737 final KnownGravityNormAccelerometerCalibrator calibrator =
4738 new KnownGravityNormAccelerometerCalibrator(measurements,
4739 true, biasX, biasY, biasZ,
4740 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4741
4742
4743 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4744 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4745 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4746 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4747 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4748 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4749 final Acceleration bx2 = new Acceleration(0.0,
4750 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4751 calibrator.getInitialBiasXAsAcceleration(bx2);
4752 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4753 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4754 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4755 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4756 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4757 final Acceleration by2 = new Acceleration(0.0,
4758 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4759 calibrator.getInitialBiasYAsAcceleration(by2);
4760 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4761 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4762 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4763 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4764 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4765 final Acceleration bz2 = new Acceleration(0.0,
4766 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4767 calibrator.getInitialBiasZAsAcceleration(bz2);
4768 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4769 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4770 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4771 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4772 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4773 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4774 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4775 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4776 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4777 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4778 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4779 final double[] bias1 = calibrator.getInitialBias();
4780 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4781 final double[] bias2 = new double[3];
4782 calibrator.getInitialBias(bias2);
4783 assertArrayEquals(bias1, bias2, 0.0);
4784 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4785 assertEquals(b1, ba);
4786 final Matrix b2 = new Matrix(3, 1);
4787 calibrator.getInitialBiasAsMatrix(b2);
4788 assertEquals(b1, b2);
4789 final Matrix ma1 = new Matrix(3, 3);
4790 ma1.setSubmatrix(0, 0,
4791 2, 2,
4792 new double[]{sx, myx, mzx,
4793 mxy, sy, mzy,
4794 mxz, myz, sz});
4795 assertEquals(calibrator.getInitialMa(), ma1);
4796 final Matrix ma2 = new Matrix(3, 3);
4797 calibrator.getInitialMa(ma2);
4798 assertEquals(ma1, ma2);
4799 assertSame(calibrator.getMeasurements(), measurements);
4800 assertTrue(calibrator.isCommonAxisUsed());
4801 assertNull(calibrator.getListener());
4802 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4803 assertFalse(calibrator.isReady());
4804 assertFalse(calibrator.isRunning());
4805 assertNull(calibrator.getEstimatedBiases());
4806 assertFalse(calibrator.getEstimatedBiases(null));
4807 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4808 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4809 assertNull(calibrator.getEstimatedBiasFx());
4810 assertNull(calibrator.getEstimatedBiasFy());
4811 assertNull(calibrator.getEstimatedBiasFz());
4812 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4813 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4814 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4815 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4816 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4817 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4818 assertNull(calibrator.getEstimatedMa());
4819 assertNull(calibrator.getEstimatedSx());
4820 assertNull(calibrator.getEstimatedSy());
4821 assertNull(calibrator.getEstimatedSz());
4822 assertNull(calibrator.getEstimatedMxy());
4823 assertNull(calibrator.getEstimatedMxz());
4824 assertNull(calibrator.getEstimatedMyx());
4825 assertNull(calibrator.getEstimatedMyz());
4826 assertNull(calibrator.getEstimatedMzx());
4827 assertNull(calibrator.getEstimatedMzy());
4828 assertNull(calibrator.getEstimatedCovariance());
4829 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4830 assertNull(calibrator.getGroundTruthGravityNorm());
4831 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4832 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4833 }
4834
4835 @Test
4836 public void testConstructor46() throws WrongSizeException {
4837 final Collection<StandardDeviationBodyKinematics> measurements =
4838 Collections.emptyList();
4839
4840 final Matrix ba = generateBa();
4841 final double biasX = ba.getElementAtIndex(0);
4842 final double biasY = ba.getElementAtIndex(1);
4843 final double biasZ = ba.getElementAtIndex(2);
4844
4845 final Matrix ma = generateMaCommonAxis();
4846 final double sx = ma.getElementAt(0, 0);
4847 final double sy = ma.getElementAt(1, 1);
4848 final double sz = ma.getElementAt(2, 2);
4849 final double mxy = ma.getElementAt(0, 1);
4850 final double mxz = ma.getElementAt(0, 2);
4851 final double myx = ma.getElementAt(1, 0);
4852 final double myz = ma.getElementAt(1, 2);
4853 final double mzx = ma.getElementAt(2, 0);
4854 final double mzy = ma.getElementAt(2, 1);
4855
4856 final KnownGravityNormAccelerometerCalibrator calibrator =
4857 new KnownGravityNormAccelerometerCalibrator(measurements,
4858 true, biasX, biasY, biasZ,
4859 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4860
4861
4862 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4863 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4864 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4865 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4866 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4867 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4868 final Acceleration bx2 = new Acceleration(0.0,
4869 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4870 calibrator.getInitialBiasXAsAcceleration(bx2);
4871 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4872 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4873 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4874 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4875 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4876 final Acceleration by2 = new Acceleration(0.0,
4877 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4878 calibrator.getInitialBiasYAsAcceleration(by2);
4879 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4880 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4881 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4882 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4883 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4884 final Acceleration bz2 = new Acceleration(0.0,
4885 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4886 calibrator.getInitialBiasZAsAcceleration(bz2);
4887 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4888 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4889 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4890 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4891 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4892 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4893 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4894 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4895 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4896 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4897 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4898 final double[] bias1 = calibrator.getInitialBias();
4899 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4900 final double[] bias2 = new double[3];
4901 calibrator.getInitialBias(bias2);
4902 assertArrayEquals(bias1, bias2, 0.0);
4903 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4904 assertEquals(b1, ba);
4905 final Matrix b2 = new Matrix(3, 1);
4906 calibrator.getInitialBiasAsMatrix(b2);
4907 assertEquals(b1, b2);
4908 final Matrix ma1 = new Matrix(3, 3);
4909 ma1.setSubmatrix(0, 0,
4910 2, 2,
4911 new double[]{sx, myx, mzx,
4912 mxy, sy, mzy,
4913 mxz, myz, sz});
4914 assertEquals(calibrator.getInitialMa(), ma1);
4915 final Matrix ma2 = new Matrix(3, 3);
4916 calibrator.getInitialMa(ma2);
4917 assertEquals(ma1, ma2);
4918 assertSame(calibrator.getMeasurements(), measurements);
4919 assertTrue(calibrator.isCommonAxisUsed());
4920 assertSame(calibrator.getListener(), this);
4921 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4922 assertFalse(calibrator.isReady());
4923 assertFalse(calibrator.isRunning());
4924 assertNull(calibrator.getEstimatedBiases());
4925 assertFalse(calibrator.getEstimatedBiases(null));
4926 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4927 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4928 assertNull(calibrator.getEstimatedBiasFx());
4929 assertNull(calibrator.getEstimatedBiasFy());
4930 assertNull(calibrator.getEstimatedBiasFz());
4931 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4932 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4933 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4934 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4935 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4936 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4937 assertNull(calibrator.getEstimatedMa());
4938 assertNull(calibrator.getEstimatedSx());
4939 assertNull(calibrator.getEstimatedSy());
4940 assertNull(calibrator.getEstimatedSz());
4941 assertNull(calibrator.getEstimatedMxy());
4942 assertNull(calibrator.getEstimatedMxz());
4943 assertNull(calibrator.getEstimatedMyx());
4944 assertNull(calibrator.getEstimatedMyz());
4945 assertNull(calibrator.getEstimatedMzx());
4946 assertNull(calibrator.getEstimatedMzy());
4947 assertNull(calibrator.getEstimatedCovariance());
4948 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4949 assertNull(calibrator.getGroundTruthGravityNorm());
4950 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4951 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4952 }
4953
4954 @Test
4955 public void testConstructor47() throws WrongSizeException {
4956 final Matrix ba = generateBa();
4957 final double biasX = ba.getElementAtIndex(0);
4958 final double biasY = ba.getElementAtIndex(1);
4959 final double biasZ = ba.getElementAtIndex(2);
4960
4961 final Matrix ma = generateMaCommonAxis();
4962 final double sx = ma.getElementAt(0, 0);
4963 final double sy = ma.getElementAt(1, 1);
4964 final double sz = ma.getElementAt(2, 2);
4965 final double mxy = ma.getElementAt(0, 1);
4966 final double mxz = ma.getElementAt(0, 2);
4967 final double myx = ma.getElementAt(1, 0);
4968 final double myz = ma.getElementAt(1, 2);
4969 final double mzx = ma.getElementAt(2, 0);
4970 final double mzy = ma.getElementAt(2, 1);
4971
4972 final Acceleration bx = new Acceleration(biasX,
4973 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4974 final Acceleration by = new Acceleration(biasY,
4975 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4976 final Acceleration bz = new Acceleration(biasZ,
4977 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4978
4979 final KnownGravityNormAccelerometerCalibrator calibrator =
4980 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
4981 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4982
4983
4984 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4985 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4986 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4987 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4988 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4989 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4990 final Acceleration bx2 = new Acceleration(0.0,
4991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4992 calibrator.getInitialBiasXAsAcceleration(bx2);
4993 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4994 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4995 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4996 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4997 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4998 final Acceleration by2 = new Acceleration(0.0,
4999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5000 calibrator.getInitialBiasYAsAcceleration(by2);
5001 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5002 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5003 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5004 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5005 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5006 final Acceleration bz2 = new Acceleration(0.0,
5007 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5008 calibrator.getInitialBiasZAsAcceleration(bz2);
5009 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5010 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5011 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5012 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5013 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5014 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5015 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5016 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5017 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5018 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5019 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5020 final double[] bias1 = calibrator.getInitialBias();
5021 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5022 final double[] bias2 = new double[3];
5023 calibrator.getInitialBias(bias2);
5024 assertArrayEquals(bias1, bias2, 0.0);
5025 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5026 assertEquals(b1, ba);
5027 final Matrix b2 = new Matrix(3, 1);
5028 calibrator.getInitialBiasAsMatrix(b2);
5029 assertEquals(b1, b2);
5030 final Matrix ma1 = new Matrix(3, 3);
5031 ma1.setSubmatrix(0, 0,
5032 2, 2,
5033 new double[]{sx, myx, mzx,
5034 mxy, sy, mzy,
5035 mxz, myz, sz});
5036 assertEquals(calibrator.getInitialMa(), ma1);
5037 final Matrix ma2 = new Matrix(3, 3);
5038 calibrator.getInitialMa(ma2);
5039 assertEquals(ma1, ma2);
5040 assertNull(calibrator.getMeasurements());
5041 assertFalse(calibrator.isCommonAxisUsed());
5042 assertNull(calibrator.getListener());
5043 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5044 assertFalse(calibrator.isReady());
5045 assertFalse(calibrator.isRunning());
5046 assertNull(calibrator.getEstimatedBiases());
5047 assertFalse(calibrator.getEstimatedBiases(null));
5048 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5049 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5050 assertNull(calibrator.getEstimatedBiasFx());
5051 assertNull(calibrator.getEstimatedBiasFy());
5052 assertNull(calibrator.getEstimatedBiasFz());
5053 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5054 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5055 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5056 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5057 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5058 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5059 assertNull(calibrator.getEstimatedMa());
5060 assertNull(calibrator.getEstimatedSx());
5061 assertNull(calibrator.getEstimatedSy());
5062 assertNull(calibrator.getEstimatedSz());
5063 assertNull(calibrator.getEstimatedMxy());
5064 assertNull(calibrator.getEstimatedMxz());
5065 assertNull(calibrator.getEstimatedMyx());
5066 assertNull(calibrator.getEstimatedMyz());
5067 assertNull(calibrator.getEstimatedMzx());
5068 assertNull(calibrator.getEstimatedMzy());
5069 assertNull(calibrator.getEstimatedCovariance());
5070 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5071 assertNull(calibrator.getGroundTruthGravityNorm());
5072 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5073 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5074 }
5075
5076 @Test
5077 public void testConstructor48() throws WrongSizeException {
5078 final Matrix ba = generateBa();
5079 final double biasX = ba.getElementAtIndex(0);
5080 final double biasY = ba.getElementAtIndex(1);
5081 final double biasZ = ba.getElementAtIndex(2);
5082
5083 final Matrix ma = generateMaCommonAxis();
5084 final double sx = ma.getElementAt(0, 0);
5085 final double sy = ma.getElementAt(1, 1);
5086 final double sz = ma.getElementAt(2, 2);
5087 final double mxy = ma.getElementAt(0, 1);
5088 final double mxz = ma.getElementAt(0, 2);
5089 final double myx = ma.getElementAt(1, 0);
5090 final double myz = ma.getElementAt(1, 2);
5091 final double mzx = ma.getElementAt(2, 0);
5092 final double mzy = ma.getElementAt(2, 1);
5093
5094 final Acceleration bx = new Acceleration(biasX,
5095 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5096 final Acceleration by = new Acceleration(biasY,
5097 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5098 final Acceleration bz = new Acceleration(biasZ,
5099 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5100
5101 final KnownGravityNormAccelerometerCalibrator calibrator =
5102 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
5103 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5104
5105
5106 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5107 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5108 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5109 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5110 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5111 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5112 final Acceleration bx2 = new Acceleration(0.0,
5113 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5114 calibrator.getInitialBiasXAsAcceleration(bx2);
5115 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5116 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5117 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5118 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5119 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5120 final Acceleration by2 = new Acceleration(0.0,
5121 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5122 calibrator.getInitialBiasYAsAcceleration(by2);
5123 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5124 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5125 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5126 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5127 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5128 final Acceleration bz2 = new Acceleration(0.0,
5129 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5130 calibrator.getInitialBiasZAsAcceleration(bz2);
5131 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5132 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5133 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5134 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5135 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5136 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5137 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5138 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5139 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5140 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5141 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5142 final double[] bias1 = calibrator.getInitialBias();
5143 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5144 final double[] bias2 = new double[3];
5145 calibrator.getInitialBias(bias2);
5146 assertArrayEquals(bias1, bias2, 0.0);
5147 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5148 assertEquals(b1, ba);
5149 final Matrix b2 = new Matrix(3, 1);
5150 calibrator.getInitialBiasAsMatrix(b2);
5151 assertEquals(b1, b2);
5152 final Matrix ma1 = new Matrix(3, 3);
5153 ma1.setSubmatrix(0, 0,
5154 2, 2,
5155 new double[]{sx, myx, mzx,
5156 mxy, sy, mzy,
5157 mxz, myz, sz});
5158 assertEquals(calibrator.getInitialMa(), ma1);
5159 final Matrix ma2 = new Matrix(3, 3);
5160 calibrator.getInitialMa(ma2);
5161 assertEquals(ma1, ma2);
5162 assertNull(calibrator.getMeasurements());
5163 assertFalse(calibrator.isCommonAxisUsed());
5164 assertSame(calibrator.getListener(), this);
5165 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5166 assertFalse(calibrator.isReady());
5167 assertFalse(calibrator.isRunning());
5168 assertNull(calibrator.getEstimatedBiases());
5169 assertFalse(calibrator.getEstimatedBiases(null));
5170 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5171 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5172 assertNull(calibrator.getEstimatedBiasFx());
5173 assertNull(calibrator.getEstimatedBiasFy());
5174 assertNull(calibrator.getEstimatedBiasFz());
5175 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5176 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5177 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5178 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5179 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5180 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5181 assertNull(calibrator.getEstimatedMa());
5182 assertNull(calibrator.getEstimatedSx());
5183 assertNull(calibrator.getEstimatedSy());
5184 assertNull(calibrator.getEstimatedSz());
5185 assertNull(calibrator.getEstimatedMxy());
5186 assertNull(calibrator.getEstimatedMxz());
5187 assertNull(calibrator.getEstimatedMyx());
5188 assertNull(calibrator.getEstimatedMyz());
5189 assertNull(calibrator.getEstimatedMzx());
5190 assertNull(calibrator.getEstimatedMzy());
5191 assertNull(calibrator.getEstimatedCovariance());
5192 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5193 assertNull(calibrator.getGroundTruthGravityNorm());
5194 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5195 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5196 }
5197
5198 @Test
5199 public void testConstructor49() throws WrongSizeException {
5200
5201 final Collection<StandardDeviationBodyKinematics> measurements =
5202 Collections.emptyList();
5203
5204 final Matrix ba = generateBa();
5205 final double biasX = ba.getElementAtIndex(0);
5206 final double biasY = ba.getElementAtIndex(1);
5207 final double biasZ = ba.getElementAtIndex(2);
5208
5209 final Matrix ma = generateMaCommonAxis();
5210 final double sx = ma.getElementAt(0, 0);
5211 final double sy = ma.getElementAt(1, 1);
5212 final double sz = ma.getElementAt(2, 2);
5213 final double mxy = ma.getElementAt(0, 1);
5214 final double mxz = ma.getElementAt(0, 2);
5215 final double myx = ma.getElementAt(1, 0);
5216 final double myz = ma.getElementAt(1, 2);
5217 final double mzx = ma.getElementAt(2, 0);
5218 final double mzy = ma.getElementAt(2, 1);
5219
5220 final Acceleration bx = new Acceleration(biasX,
5221 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5222 final Acceleration by = new Acceleration(biasY,
5223 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5224 final Acceleration bz = new Acceleration(biasZ,
5225 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5226
5227 final KnownGravityNormAccelerometerCalibrator calibrator =
5228 new KnownGravityNormAccelerometerCalibrator(
5229 measurements, bx, by, bz, sx, sy, sz,
5230 mxy, mxz, myx, myz, mzx, mzy);
5231
5232
5233 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5234 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5235 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5236 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5237 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5238 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5239 final Acceleration bx2 = new Acceleration(0.0,
5240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5241 calibrator.getInitialBiasXAsAcceleration(bx2);
5242 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5243 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5244 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5245 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5246 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5247 final Acceleration by2 = new Acceleration(0.0,
5248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5249 calibrator.getInitialBiasYAsAcceleration(by2);
5250 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5251 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5252 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5253 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5254 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5255 final Acceleration bz2 = new Acceleration(0.0,
5256 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5257 calibrator.getInitialBiasZAsAcceleration(bz2);
5258 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5259 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5260 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5261 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5262 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5263 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5264 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5265 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5266 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5267 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5268 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5269 final double[] bias1 = calibrator.getInitialBias();
5270 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5271 final double[] bias2 = new double[3];
5272 calibrator.getInitialBias(bias2);
5273 assertArrayEquals(bias1, bias2, 0.0);
5274 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5275 assertEquals(b1, ba);
5276 final Matrix b2 = new Matrix(3, 1);
5277 calibrator.getInitialBiasAsMatrix(b2);
5278 assertEquals(b1, b2);
5279 final Matrix ma1 = new Matrix(3, 3);
5280 ma1.setSubmatrix(0, 0,
5281 2, 2,
5282 new double[]{sx, myx, mzx,
5283 mxy, sy, mzy,
5284 mxz, myz, sz});
5285 assertEquals(calibrator.getInitialMa(), ma1);
5286 final Matrix ma2 = new Matrix(3, 3);
5287 calibrator.getInitialMa(ma2);
5288 assertEquals(ma1, ma2);
5289 assertSame(calibrator.getMeasurements(), measurements);
5290 assertFalse(calibrator.isCommonAxisUsed());
5291 assertNull(calibrator.getListener());
5292 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5293 assertFalse(calibrator.isReady());
5294 assertFalse(calibrator.isRunning());
5295 assertNull(calibrator.getEstimatedBiases());
5296 assertFalse(calibrator.getEstimatedBiases(null));
5297 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5298 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5299 assertNull(calibrator.getEstimatedBiasFx());
5300 assertNull(calibrator.getEstimatedBiasFy());
5301 assertNull(calibrator.getEstimatedBiasFz());
5302 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5303 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5304 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5305 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5306 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5307 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5308 assertNull(calibrator.getEstimatedMa());
5309 assertNull(calibrator.getEstimatedSx());
5310 assertNull(calibrator.getEstimatedSy());
5311 assertNull(calibrator.getEstimatedSz());
5312 assertNull(calibrator.getEstimatedMxy());
5313 assertNull(calibrator.getEstimatedMxz());
5314 assertNull(calibrator.getEstimatedMyx());
5315 assertNull(calibrator.getEstimatedMyz());
5316 assertNull(calibrator.getEstimatedMzx());
5317 assertNull(calibrator.getEstimatedMzy());
5318 assertNull(calibrator.getEstimatedCovariance());
5319 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5320 assertNull(calibrator.getGroundTruthGravityNorm());
5321 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5322 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5323 }
5324
5325 @Test
5326 public void testConstructor50() throws WrongSizeException {
5327
5328 final Collection<StandardDeviationBodyKinematics> measurements =
5329 Collections.emptyList();
5330
5331 final Matrix ba = generateBa();
5332 final double biasX = ba.getElementAtIndex(0);
5333 final double biasY = ba.getElementAtIndex(1);
5334 final double biasZ = ba.getElementAtIndex(2);
5335
5336 final Matrix ma = generateMaCommonAxis();
5337 final double sx = ma.getElementAt(0, 0);
5338 final double sy = ma.getElementAt(1, 1);
5339 final double sz = ma.getElementAt(2, 2);
5340 final double mxy = ma.getElementAt(0, 1);
5341 final double mxz = ma.getElementAt(0, 2);
5342 final double myx = ma.getElementAt(1, 0);
5343 final double myz = ma.getElementAt(1, 2);
5344 final double mzx = ma.getElementAt(2, 0);
5345 final double mzy = ma.getElementAt(2, 1);
5346
5347 final Acceleration bx = new Acceleration(biasX,
5348 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5349 final Acceleration by = new Acceleration(biasY,
5350 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5351 final Acceleration bz = new Acceleration(biasZ,
5352 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5353
5354 final KnownGravityNormAccelerometerCalibrator calibrator =
5355 new KnownGravityNormAccelerometerCalibrator(
5356 measurements, bx, by, bz, sx, sy, sz,
5357 mxy, mxz, myx, myz, mzx, mzy, this);
5358
5359
5360 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5361 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5362 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5363 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5364 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5365 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5366 final Acceleration bx2 = new Acceleration(0.0,
5367 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5368 calibrator.getInitialBiasXAsAcceleration(bx2);
5369 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5370 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5371 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5372 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5373 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5374 final Acceleration by2 = new Acceleration(0.0,
5375 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5376 calibrator.getInitialBiasYAsAcceleration(by2);
5377 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5378 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5379 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5380 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5381 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382 final Acceleration bz2 = new Acceleration(0.0,
5383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5384 calibrator.getInitialBiasZAsAcceleration(bz2);
5385 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5386 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5387 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5388 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5389 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5390 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5391 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5392 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5393 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5394 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5395 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5396 final double[] bias1 = calibrator.getInitialBias();
5397 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5398 final double[] bias2 = new double[3];
5399 calibrator.getInitialBias(bias2);
5400 assertArrayEquals(bias1, bias2, 0.0);
5401 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5402 assertEquals(b1, ba);
5403 final Matrix b2 = new Matrix(3, 1);
5404 calibrator.getInitialBiasAsMatrix(b2);
5405 assertEquals(b1, b2);
5406 final Matrix ma1 = new Matrix(3, 3);
5407 ma1.setSubmatrix(0, 0,
5408 2, 2,
5409 new double[]{sx, myx, mzx,
5410 mxy, sy, mzy,
5411 mxz, myz, sz});
5412 assertEquals(calibrator.getInitialMa(), ma1);
5413 final Matrix ma2 = new Matrix(3, 3);
5414 calibrator.getInitialMa(ma2);
5415 assertEquals(ma1, ma2);
5416 assertSame(calibrator.getMeasurements(), measurements);
5417 assertFalse(calibrator.isCommonAxisUsed());
5418 assertSame(calibrator.getListener(), this);
5419 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5420 assertFalse(calibrator.isReady());
5421 assertFalse(calibrator.isRunning());
5422 assertNull(calibrator.getEstimatedBiases());
5423 assertFalse(calibrator.getEstimatedBiases(null));
5424 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5425 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5426 assertNull(calibrator.getEstimatedBiasFx());
5427 assertNull(calibrator.getEstimatedBiasFy());
5428 assertNull(calibrator.getEstimatedBiasFz());
5429 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5430 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5431 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5432 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5433 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5434 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5435 assertNull(calibrator.getEstimatedMa());
5436 assertNull(calibrator.getEstimatedSx());
5437 assertNull(calibrator.getEstimatedSy());
5438 assertNull(calibrator.getEstimatedSz());
5439 assertNull(calibrator.getEstimatedMxy());
5440 assertNull(calibrator.getEstimatedMxz());
5441 assertNull(calibrator.getEstimatedMyx());
5442 assertNull(calibrator.getEstimatedMyz());
5443 assertNull(calibrator.getEstimatedMzx());
5444 assertNull(calibrator.getEstimatedMzy());
5445 assertNull(calibrator.getEstimatedCovariance());
5446 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5447 assertNull(calibrator.getGroundTruthGravityNorm());
5448 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5449 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5450 }
5451
5452 @Test
5453 public void testConstructor51() throws WrongSizeException {
5454 final Matrix ba = generateBa();
5455 final double biasX = ba.getElementAtIndex(0);
5456 final double biasY = ba.getElementAtIndex(1);
5457 final double biasZ = ba.getElementAtIndex(2);
5458
5459 final Matrix ma = generateMaCommonAxis();
5460 final double sx = ma.getElementAt(0, 0);
5461 final double sy = ma.getElementAt(1, 1);
5462 final double sz = ma.getElementAt(2, 2);
5463 final double mxy = ma.getElementAt(0, 1);
5464 final double mxz = ma.getElementAt(0, 2);
5465 final double myx = ma.getElementAt(1, 0);
5466 final double myz = ma.getElementAt(1, 2);
5467 final double mzx = ma.getElementAt(2, 0);
5468 final double mzy = ma.getElementAt(2, 1);
5469
5470 final Acceleration bx = new Acceleration(biasX,
5471 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5472 final Acceleration by = new Acceleration(biasY,
5473 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5474 final Acceleration bz = new Acceleration(biasZ,
5475 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5476
5477 final KnownGravityNormAccelerometerCalibrator calibrator =
5478 new KnownGravityNormAccelerometerCalibrator(
5479 true, bx, by, bz, sx, sy, sz,
5480 mxy, mxz, myx, myz, mzx, mzy);
5481
5482
5483 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5484 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5485 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5486 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5487 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5488 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5489 final Acceleration bx2 = new Acceleration(0.0,
5490 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5491 calibrator.getInitialBiasXAsAcceleration(bx2);
5492 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5493 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5494 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5495 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5496 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5497 final Acceleration by2 = new Acceleration(0.0,
5498 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5499 calibrator.getInitialBiasYAsAcceleration(by2);
5500 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5501 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5502 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5503 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5504 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505 final Acceleration bz2 = new Acceleration(0.0,
5506 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5507 calibrator.getInitialBiasZAsAcceleration(bz2);
5508 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5509 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5510 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5511 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5512 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5513 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5514 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5515 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5516 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5517 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5518 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5519 final double[] bias1 = calibrator.getInitialBias();
5520 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5521 final double[] bias2 = new double[3];
5522 calibrator.getInitialBias(bias2);
5523 assertArrayEquals(bias1, bias2, 0.0);
5524 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5525 assertEquals(b1, ba);
5526 final Matrix b2 = new Matrix(3, 1);
5527 calibrator.getInitialBiasAsMatrix(b2);
5528 assertEquals(b1, b2);
5529 final Matrix ma1 = new Matrix(3, 3);
5530 ma1.setSubmatrix(0, 0,
5531 2, 2,
5532 new double[]{sx, myx, mzx,
5533 mxy, sy, mzy,
5534 mxz, myz, sz});
5535 assertEquals(calibrator.getInitialMa(), ma1);
5536 final Matrix ma2 = new Matrix(3, 3);
5537 calibrator.getInitialMa(ma2);
5538 assertEquals(ma1, ma2);
5539 assertNull(calibrator.getMeasurements());
5540 assertTrue(calibrator.isCommonAxisUsed());
5541 assertNull(calibrator.getListener());
5542 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5543 assertFalse(calibrator.isReady());
5544 assertFalse(calibrator.isRunning());
5545 assertNull(calibrator.getEstimatedBiases());
5546 assertFalse(calibrator.getEstimatedBiases(null));
5547 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5548 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5549 assertNull(calibrator.getEstimatedBiasFx());
5550 assertNull(calibrator.getEstimatedBiasFy());
5551 assertNull(calibrator.getEstimatedBiasFz());
5552 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5553 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5554 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5555 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5556 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5557 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5558 assertNull(calibrator.getEstimatedMa());
5559 assertNull(calibrator.getEstimatedSx());
5560 assertNull(calibrator.getEstimatedSy());
5561 assertNull(calibrator.getEstimatedSz());
5562 assertNull(calibrator.getEstimatedMxy());
5563 assertNull(calibrator.getEstimatedMxz());
5564 assertNull(calibrator.getEstimatedMyx());
5565 assertNull(calibrator.getEstimatedMyz());
5566 assertNull(calibrator.getEstimatedMzx());
5567 assertNull(calibrator.getEstimatedMzy());
5568 assertNull(calibrator.getEstimatedCovariance());
5569 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5570 assertNull(calibrator.getGroundTruthGravityNorm());
5571 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5572 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5573 }
5574
5575 @Test
5576 public void testConstructor52() throws WrongSizeException {
5577 final Matrix ba = generateBa();
5578 final double biasX = ba.getElementAtIndex(0);
5579 final double biasY = ba.getElementAtIndex(1);
5580 final double biasZ = ba.getElementAtIndex(2);
5581
5582 final Matrix ma = generateMaCommonAxis();
5583 final double sx = ma.getElementAt(0, 0);
5584 final double sy = ma.getElementAt(1, 1);
5585 final double sz = ma.getElementAt(2, 2);
5586 final double mxy = ma.getElementAt(0, 1);
5587 final double mxz = ma.getElementAt(0, 2);
5588 final double myx = ma.getElementAt(1, 0);
5589 final double myz = ma.getElementAt(1, 2);
5590 final double mzx = ma.getElementAt(2, 0);
5591 final double mzy = ma.getElementAt(2, 1);
5592
5593 final Acceleration bx = new Acceleration(biasX,
5594 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5595 final Acceleration by = new Acceleration(biasY,
5596 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5597 final Acceleration bz = new Acceleration(biasZ,
5598 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5599
5600 final KnownGravityNormAccelerometerCalibrator calibrator =
5601 new KnownGravityNormAccelerometerCalibrator(true,
5602 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5603 this);
5604
5605
5606 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5607 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5608 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5609 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5610 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5611 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5612 final Acceleration bx2 = new Acceleration(0.0,
5613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5614 calibrator.getInitialBiasXAsAcceleration(bx2);
5615 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5616 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5617 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5618 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5619 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5620 final Acceleration by2 = new Acceleration(0.0,
5621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5622 calibrator.getInitialBiasYAsAcceleration(by2);
5623 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5624 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5625 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5626 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5627 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5628 final Acceleration bz2 = new Acceleration(0.0,
5629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5630 calibrator.getInitialBiasZAsAcceleration(bz2);
5631 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5632 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5633 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5634 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5635 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5636 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5637 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5638 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5639 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5640 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5641 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5642 final double[] bias1 = calibrator.getInitialBias();
5643 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5644 final double[] bias2 = new double[3];
5645 calibrator.getInitialBias(bias2);
5646 assertArrayEquals(bias1, bias2, 0.0);
5647 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5648 assertEquals(b1, ba);
5649 final Matrix b2 = new Matrix(3, 1);
5650 calibrator.getInitialBiasAsMatrix(b2);
5651 assertEquals(b1, b2);
5652 final Matrix ma1 = new Matrix(3, 3);
5653 ma1.setSubmatrix(0, 0,
5654 2, 2,
5655 new double[]{sx, myx, mzx,
5656 mxy, sy, mzy,
5657 mxz, myz, sz});
5658 assertEquals(calibrator.getInitialMa(), ma1);
5659 final Matrix ma2 = new Matrix(3, 3);
5660 calibrator.getInitialMa(ma2);
5661 assertEquals(ma1, ma2);
5662 assertNull(calibrator.getMeasurements());
5663 assertTrue(calibrator.isCommonAxisUsed());
5664 assertSame(calibrator.getListener(), this);
5665 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5666 assertFalse(calibrator.isReady());
5667 assertFalse(calibrator.isRunning());
5668 assertNull(calibrator.getEstimatedBiases());
5669 assertFalse(calibrator.getEstimatedBiases(null));
5670 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5671 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5672 assertNull(calibrator.getEstimatedBiasFx());
5673 assertNull(calibrator.getEstimatedBiasFy());
5674 assertNull(calibrator.getEstimatedBiasFz());
5675 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5676 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5677 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5678 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5679 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5680 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5681 assertNull(calibrator.getEstimatedMa());
5682 assertNull(calibrator.getEstimatedSx());
5683 assertNull(calibrator.getEstimatedSy());
5684 assertNull(calibrator.getEstimatedSz());
5685 assertNull(calibrator.getEstimatedMxy());
5686 assertNull(calibrator.getEstimatedMxz());
5687 assertNull(calibrator.getEstimatedMyx());
5688 assertNull(calibrator.getEstimatedMyz());
5689 assertNull(calibrator.getEstimatedMzx());
5690 assertNull(calibrator.getEstimatedMzy());
5691 assertNull(calibrator.getEstimatedCovariance());
5692 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5693 assertNull(calibrator.getGroundTruthGravityNorm());
5694 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5695 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5696 }
5697
5698 @Test
5699 public void testConstructor53() throws WrongSizeException {
5700 final Collection<StandardDeviationBodyKinematics> measurements =
5701 Collections.emptyList();
5702
5703 final Matrix ba = generateBa();
5704 final double biasX = ba.getElementAtIndex(0);
5705 final double biasY = ba.getElementAtIndex(1);
5706 final double biasZ = ba.getElementAtIndex(2);
5707
5708 final Matrix ma = generateMaCommonAxis();
5709 final double sx = ma.getElementAt(0, 0);
5710 final double sy = ma.getElementAt(1, 1);
5711 final double sz = ma.getElementAt(2, 2);
5712 final double mxy = ma.getElementAt(0, 1);
5713 final double mxz = ma.getElementAt(0, 2);
5714 final double myx = ma.getElementAt(1, 0);
5715 final double myz = ma.getElementAt(1, 2);
5716 final double mzx = ma.getElementAt(2, 0);
5717 final double mzy = ma.getElementAt(2, 1);
5718
5719 final Acceleration bx = new Acceleration(biasX,
5720 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5721 final Acceleration by = new Acceleration(biasY,
5722 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5723 final Acceleration bz = new Acceleration(biasZ,
5724 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5725
5726 final KnownGravityNormAccelerometerCalibrator calibrator =
5727 new KnownGravityNormAccelerometerCalibrator(measurements,
5728 true, bx, by, bz, sx, sy, sz,
5729 mxy, mxz, myx, myz, mzx, mzy);
5730
5731
5732 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5733 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5734 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5735 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5736 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5737 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5738 final Acceleration bx2 = new Acceleration(0.0,
5739 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5740 calibrator.getInitialBiasXAsAcceleration(bx2);
5741 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5742 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5743 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5744 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5745 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5746 final Acceleration by2 = new Acceleration(0.0,
5747 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5748 calibrator.getInitialBiasYAsAcceleration(by2);
5749 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5750 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5751 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5752 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5753 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5754 final Acceleration bz2 = new Acceleration(0.0,
5755 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5756 calibrator.getInitialBiasZAsAcceleration(bz2);
5757 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5758 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5759 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5760 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5761 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5762 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5763 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5764 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5765 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5766 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5767 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5768 final double[] bias1 = calibrator.getInitialBias();
5769 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5770 final double[] bias2 = new double[3];
5771 calibrator.getInitialBias(bias2);
5772 assertArrayEquals(bias1, bias2, 0.0);
5773 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5774 assertEquals(b1, ba);
5775 final Matrix b2 = new Matrix(3, 1);
5776 calibrator.getInitialBiasAsMatrix(b2);
5777 assertEquals(b1, b2);
5778 final Matrix ma1 = new Matrix(3, 3);
5779 ma1.setSubmatrix(0, 0,
5780 2, 2,
5781 new double[]{sx, myx, mzx,
5782 mxy, sy, mzy,
5783 mxz, myz, sz});
5784 assertEquals(calibrator.getInitialMa(), ma1);
5785 final Matrix ma2 = new Matrix(3, 3);
5786 calibrator.getInitialMa(ma2);
5787 assertEquals(ma1, ma2);
5788 assertSame(calibrator.getMeasurements(), measurements);
5789 assertTrue(calibrator.isCommonAxisUsed());
5790 assertNull(calibrator.getListener());
5791 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5792 assertFalse(calibrator.isReady());
5793 assertFalse(calibrator.isRunning());
5794 assertNull(calibrator.getEstimatedBiases());
5795 assertFalse(calibrator.getEstimatedBiases(null));
5796 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5797 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5798 assertNull(calibrator.getEstimatedBiasFx());
5799 assertNull(calibrator.getEstimatedBiasFy());
5800 assertNull(calibrator.getEstimatedBiasFz());
5801 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5802 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5803 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5804 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5805 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5806 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5807 assertNull(calibrator.getEstimatedMa());
5808 assertNull(calibrator.getEstimatedSx());
5809 assertNull(calibrator.getEstimatedSy());
5810 assertNull(calibrator.getEstimatedSz());
5811 assertNull(calibrator.getEstimatedMxy());
5812 assertNull(calibrator.getEstimatedMxz());
5813 assertNull(calibrator.getEstimatedMyx());
5814 assertNull(calibrator.getEstimatedMyz());
5815 assertNull(calibrator.getEstimatedMzx());
5816 assertNull(calibrator.getEstimatedMzy());
5817 assertNull(calibrator.getEstimatedCovariance());
5818 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5819 assertNull(calibrator.getGroundTruthGravityNorm());
5820 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5821 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5822 }
5823
5824 @Test
5825 public void testConstructor54() throws WrongSizeException {
5826 final Collection<StandardDeviationBodyKinematics> measurements =
5827 Collections.emptyList();
5828
5829 final Matrix ba = generateBa();
5830 final double biasX = ba.getElementAtIndex(0);
5831 final double biasY = ba.getElementAtIndex(1);
5832 final double biasZ = ba.getElementAtIndex(2);
5833
5834 final Matrix ma = generateMaCommonAxis();
5835 final double sx = ma.getElementAt(0, 0);
5836 final double sy = ma.getElementAt(1, 1);
5837 final double sz = ma.getElementAt(2, 2);
5838 final double mxy = ma.getElementAt(0, 1);
5839 final double mxz = ma.getElementAt(0, 2);
5840 final double myx = ma.getElementAt(1, 0);
5841 final double myz = ma.getElementAt(1, 2);
5842 final double mzx = ma.getElementAt(2, 0);
5843 final double mzy = ma.getElementAt(2, 1);
5844
5845 final Acceleration bx = new Acceleration(biasX,
5846 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5847 final Acceleration by = new Acceleration(biasY,
5848 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5849 final Acceleration bz = new Acceleration(biasZ,
5850 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5851
5852 final KnownGravityNormAccelerometerCalibrator calibrator =
5853 new KnownGravityNormAccelerometerCalibrator(measurements,
5854 true, bx, by, bz, sx, sy, sz,
5855 mxy, mxz, myx, myz, mzx, mzy, this);
5856
5857
5858 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5859 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5860 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5861 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5862 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5863 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5864 final Acceleration bx2 = new Acceleration(0.0,
5865 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5866 calibrator.getInitialBiasXAsAcceleration(bx2);
5867 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5868 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5869 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5870 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5871 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5872 final Acceleration by2 = new Acceleration(0.0,
5873 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5874 calibrator.getInitialBiasYAsAcceleration(by2);
5875 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5876 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5877 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5878 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5879 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5880 final Acceleration bz2 = new Acceleration(0.0,
5881 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5882 calibrator.getInitialBiasZAsAcceleration(bz2);
5883 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5884 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5885 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5886 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5887 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5888 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5889 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5890 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5891 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5892 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5893 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5894 final double[] bias1 = calibrator.getInitialBias();
5895 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5896 final double[] bias2 = new double[3];
5897 calibrator.getInitialBias(bias2);
5898 assertArrayEquals(bias1, bias2, 0.0);
5899 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5900 assertEquals(b1, ba);
5901 final Matrix b2 = new Matrix(3, 1);
5902 calibrator.getInitialBiasAsMatrix(b2);
5903 assertEquals(b1, b2);
5904 final Matrix ma1 = new Matrix(3, 3);
5905 ma1.setSubmatrix(0, 0,
5906 2, 2,
5907 new double[]{sx, myx, mzx,
5908 mxy, sy, mzy,
5909 mxz, myz, sz});
5910 assertEquals(calibrator.getInitialMa(), ma1);
5911 final Matrix ma2 = new Matrix(3, 3);
5912 calibrator.getInitialMa(ma2);
5913 assertEquals(ma1, ma2);
5914 assertSame(calibrator.getMeasurements(), measurements);
5915 assertTrue(calibrator.isCommonAxisUsed());
5916 assertSame(calibrator.getListener(), this);
5917 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5918 assertFalse(calibrator.isReady());
5919 assertFalse(calibrator.isRunning());
5920 assertNull(calibrator.getEstimatedBiases());
5921 assertFalse(calibrator.getEstimatedBiases(null));
5922 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5923 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5924 assertNull(calibrator.getEstimatedBiasFx());
5925 assertNull(calibrator.getEstimatedBiasFy());
5926 assertNull(calibrator.getEstimatedBiasFz());
5927 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5928 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5929 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5930 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5931 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5932 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5933 assertNull(calibrator.getEstimatedMa());
5934 assertNull(calibrator.getEstimatedSx());
5935 assertNull(calibrator.getEstimatedSy());
5936 assertNull(calibrator.getEstimatedSz());
5937 assertNull(calibrator.getEstimatedMxy());
5938 assertNull(calibrator.getEstimatedMxz());
5939 assertNull(calibrator.getEstimatedMyx());
5940 assertNull(calibrator.getEstimatedMyz());
5941 assertNull(calibrator.getEstimatedMzx());
5942 assertNull(calibrator.getEstimatedMzy());
5943 assertNull(calibrator.getEstimatedCovariance());
5944 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5945 assertNull(calibrator.getGroundTruthGravityNorm());
5946 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5947 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5948 }
5949
5950 @Test
5951 public void testConstructor55() throws WrongSizeException {
5952 final Matrix ba = generateBa();
5953 final double[] bias = ba.getBuffer();
5954 final double biasX = ba.getElementAtIndex(0);
5955 final double biasY = ba.getElementAtIndex(1);
5956 final double biasZ = ba.getElementAtIndex(2);
5957
5958 KnownGravityNormAccelerometerCalibrator calibrator =
5959 new KnownGravityNormAccelerometerCalibrator(bias);
5960
5961
5962 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5963 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5964 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5965 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5966 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5967 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5968 final Acceleration bx2 = new Acceleration(0.0,
5969 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5970 calibrator.getInitialBiasXAsAcceleration(bx2);
5971 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5972 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5973 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5974 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5975 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5976 final Acceleration by2 = new Acceleration(0.0,
5977 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5978 calibrator.getInitialBiasYAsAcceleration(by2);
5979 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5980 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5981 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5982 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5983 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5984 final Acceleration bz2 = new Acceleration(0.0,
5985 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5986 calibrator.getInitialBiasZAsAcceleration(bz2);
5987 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5988 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5989 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5990 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5991 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5992 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5993 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5994 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5995 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5996 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5997 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5998 final double[] bias1 = calibrator.getInitialBias();
5999 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6000 final double[] bias2 = new double[3];
6001 calibrator.getInitialBias(bias2);
6002 assertArrayEquals(bias1, bias2, 0.0);
6003 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6004 assertEquals(b1, ba);
6005 final Matrix b2 = new Matrix(3, 1);
6006 calibrator.getInitialBiasAsMatrix(b2);
6007 assertEquals(b1, b2);
6008 final Matrix ma1 = calibrator.getInitialMa();
6009 assertEquals(ma1, new Matrix(3, 3));
6010 final Matrix ma2 = new Matrix(3, 3);
6011 calibrator.getInitialMa(ma2);
6012 assertEquals(ma1, ma2);
6013 assertNull(calibrator.getMeasurements());
6014 assertFalse(calibrator.isCommonAxisUsed());
6015 assertNull(calibrator.getListener());
6016 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6017 assertFalse(calibrator.isReady());
6018 assertFalse(calibrator.isRunning());
6019 assertNull(calibrator.getEstimatedBiases());
6020 assertFalse(calibrator.getEstimatedBiases(null));
6021 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6022 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6023 assertNull(calibrator.getEstimatedBiasFx());
6024 assertNull(calibrator.getEstimatedBiasFy());
6025 assertNull(calibrator.getEstimatedBiasFz());
6026 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6027 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6028 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6029 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6030 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6031 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6032 assertNull(calibrator.getEstimatedMa());
6033 assertNull(calibrator.getEstimatedSx());
6034 assertNull(calibrator.getEstimatedSy());
6035 assertNull(calibrator.getEstimatedSz());
6036 assertNull(calibrator.getEstimatedMxy());
6037 assertNull(calibrator.getEstimatedMxz());
6038 assertNull(calibrator.getEstimatedMyx());
6039 assertNull(calibrator.getEstimatedMyz());
6040 assertNull(calibrator.getEstimatedMzx());
6041 assertNull(calibrator.getEstimatedMzy());
6042 assertNull(calibrator.getEstimatedCovariance());
6043 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6044 assertNull(calibrator.getGroundTruthGravityNorm());
6045 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6046 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6047
6048
6049 calibrator = null;
6050 try {
6051 calibrator = new KnownGravityNormAccelerometerCalibrator(
6052 new double[1]);
6053 fail("IllegalArgumentException expected but not thrown");
6054 } catch (final IllegalArgumentException ignore) {
6055 }
6056 assertNull(calibrator);
6057 }
6058
6059 @Test
6060 public void testConstructor56() throws WrongSizeException {
6061 final Matrix ba = generateBa();
6062 final double[] bias = ba.getBuffer();
6063 final double biasX = ba.getElementAtIndex(0);
6064 final double biasY = ba.getElementAtIndex(1);
6065 final double biasZ = ba.getElementAtIndex(2);
6066
6067 KnownGravityNormAccelerometerCalibrator calibrator =
6068 new KnownGravityNormAccelerometerCalibrator(bias,
6069 this);
6070
6071
6072 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6073 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6074 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6075 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6076 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6077 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6078 final Acceleration bx2 = new Acceleration(0.0,
6079 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6080 calibrator.getInitialBiasXAsAcceleration(bx2);
6081 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6082 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6083 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6084 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6085 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6086 final Acceleration by2 = new Acceleration(0.0,
6087 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6088 calibrator.getInitialBiasYAsAcceleration(by2);
6089 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6090 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6091 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6092 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6093 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6094 final Acceleration bz2 = new Acceleration(0.0,
6095 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6096 calibrator.getInitialBiasZAsAcceleration(bz2);
6097 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6098 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6099 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6100 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6101 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6102 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6103 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6104 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6105 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6106 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6107 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6108 final double[] bias1 = calibrator.getInitialBias();
6109 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6110 final double[] bias2 = new double[3];
6111 calibrator.getInitialBias(bias2);
6112 assertArrayEquals(bias1, bias2, 0.0);
6113 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6114 assertEquals(b1, ba);
6115 final Matrix b2 = new Matrix(3, 1);
6116 calibrator.getInitialBiasAsMatrix(b2);
6117 assertEquals(b1, b2);
6118 final Matrix ma1 = calibrator.getInitialMa();
6119 assertEquals(ma1, new Matrix(3, 3));
6120 final Matrix ma2 = new Matrix(3, 3);
6121 calibrator.getInitialMa(ma2);
6122 assertEquals(ma1, ma2);
6123 assertNull(calibrator.getMeasurements());
6124 assertFalse(calibrator.isCommonAxisUsed());
6125 assertSame(calibrator.getListener(), this);
6126 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6127 assertFalse(calibrator.isReady());
6128 assertFalse(calibrator.isRunning());
6129 assertNull(calibrator.getEstimatedBiases());
6130 assertFalse(calibrator.getEstimatedBiases(null));
6131 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6132 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6133 assertNull(calibrator.getEstimatedBiasFx());
6134 assertNull(calibrator.getEstimatedBiasFy());
6135 assertNull(calibrator.getEstimatedBiasFz());
6136 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6137 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6138 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6139 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6140 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6141 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6142 assertNull(calibrator.getEstimatedMa());
6143 assertNull(calibrator.getEstimatedSx());
6144 assertNull(calibrator.getEstimatedSy());
6145 assertNull(calibrator.getEstimatedSz());
6146 assertNull(calibrator.getEstimatedMxy());
6147 assertNull(calibrator.getEstimatedMxz());
6148 assertNull(calibrator.getEstimatedMyx());
6149 assertNull(calibrator.getEstimatedMyz());
6150 assertNull(calibrator.getEstimatedMzx());
6151 assertNull(calibrator.getEstimatedMzy());
6152 assertNull(calibrator.getEstimatedCovariance());
6153 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6154 assertNull(calibrator.getGroundTruthGravityNorm());
6155 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6156 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6157
6158
6159 calibrator = null;
6160 try {
6161 calibrator = new KnownGravityNormAccelerometerCalibrator(
6162 new double[1], this);
6163 fail("IllegalArgumentException expected but not thrown");
6164 } catch (final IllegalArgumentException ignore) {
6165 }
6166 assertNull(calibrator);
6167 }
6168
6169 @Test
6170 public void testConstructor57() throws WrongSizeException {
6171 final Collection<StandardDeviationBodyKinematics> measurements =
6172 Collections.emptyList();
6173
6174 final Matrix ba = generateBa();
6175 final double[] bias = ba.getBuffer();
6176 final double biasX = ba.getElementAtIndex(0);
6177 final double biasY = ba.getElementAtIndex(1);
6178 final double biasZ = ba.getElementAtIndex(2);
6179
6180 KnownGravityNormAccelerometerCalibrator calibrator =
6181 new KnownGravityNormAccelerometerCalibrator(
6182 measurements, bias);
6183
6184
6185 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6186 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6187 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6188 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6189 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6190 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6191 final Acceleration bx2 = new Acceleration(0.0,
6192 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6193 calibrator.getInitialBiasXAsAcceleration(bx2);
6194 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6195 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6196 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6197 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6198 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6199 final Acceleration by2 = new Acceleration(0.0,
6200 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6201 calibrator.getInitialBiasYAsAcceleration(by2);
6202 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6203 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6204 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6205 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6206 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6207 final Acceleration bz2 = new Acceleration(0.0,
6208 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6209 calibrator.getInitialBiasZAsAcceleration(bz2);
6210 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6211 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6212 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6213 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6214 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6215 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6216 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6217 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6218 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6219 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6220 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6221 final double[] bias1 = calibrator.getInitialBias();
6222 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6223 final double[] bias2 = new double[3];
6224 calibrator.getInitialBias(bias2);
6225 assertArrayEquals(bias1, bias2, 0.0);
6226 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6227 assertEquals(b1, ba);
6228 final Matrix b2 = new Matrix(3, 1);
6229 calibrator.getInitialBiasAsMatrix(b2);
6230 assertEquals(b1, b2);
6231 final Matrix ma1 = calibrator.getInitialMa();
6232 assertEquals(ma1, new Matrix(3, 3));
6233 final Matrix ma2 = new Matrix(3, 3);
6234 calibrator.getInitialMa(ma2);
6235 assertEquals(ma1, ma2);
6236 assertSame(calibrator.getMeasurements(), measurements);
6237 assertFalse(calibrator.isCommonAxisUsed());
6238 assertNull(calibrator.getListener());
6239 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6240 assertFalse(calibrator.isReady());
6241 assertFalse(calibrator.isRunning());
6242 assertNull(calibrator.getEstimatedBiases());
6243 assertFalse(calibrator.getEstimatedBiases(null));
6244 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6245 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6246 assertNull(calibrator.getEstimatedBiasFx());
6247 assertNull(calibrator.getEstimatedBiasFy());
6248 assertNull(calibrator.getEstimatedBiasFz());
6249 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6250 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6251 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6252 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6253 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6254 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6255 assertNull(calibrator.getEstimatedMa());
6256 assertNull(calibrator.getEstimatedSx());
6257 assertNull(calibrator.getEstimatedSy());
6258 assertNull(calibrator.getEstimatedSz());
6259 assertNull(calibrator.getEstimatedMxy());
6260 assertNull(calibrator.getEstimatedMxz());
6261 assertNull(calibrator.getEstimatedMyx());
6262 assertNull(calibrator.getEstimatedMyz());
6263 assertNull(calibrator.getEstimatedMzx());
6264 assertNull(calibrator.getEstimatedMzy());
6265 assertNull(calibrator.getEstimatedCovariance());
6266 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6267 assertNull(calibrator.getGroundTruthGravityNorm());
6268 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6269 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6270
6271
6272 calibrator = null;
6273 try {
6274 calibrator = new KnownGravityNormAccelerometerCalibrator(
6275 measurements, new double[1]);
6276 fail("IllegalArgumentException expected but not thrown");
6277 } catch (final IllegalArgumentException ignore) {
6278 }
6279 assertNull(calibrator);
6280 }
6281
6282 @Test
6283 public void testConstructor58() throws WrongSizeException {
6284 final Collection<StandardDeviationBodyKinematics> measurements =
6285 Collections.emptyList();
6286
6287 final Matrix ba = generateBa();
6288 final double[] bias = ba.getBuffer();
6289 final double biasX = ba.getElementAtIndex(0);
6290 final double biasY = ba.getElementAtIndex(1);
6291 final double biasZ = ba.getElementAtIndex(2);
6292
6293 KnownGravityNormAccelerometerCalibrator calibrator =
6294 new KnownGravityNormAccelerometerCalibrator(
6295 measurements, bias, this);
6296
6297
6298 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6299 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6300 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6301 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6302 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6303 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6304 final Acceleration bx2 = new Acceleration(0.0,
6305 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6306 calibrator.getInitialBiasXAsAcceleration(bx2);
6307 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6308 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6309 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6310 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6311 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6312 final Acceleration by2 = new Acceleration(0.0,
6313 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6314 calibrator.getInitialBiasYAsAcceleration(by2);
6315 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6316 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6317 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6318 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6319 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6320 final Acceleration bz2 = new Acceleration(0.0,
6321 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6322 calibrator.getInitialBiasZAsAcceleration(bz2);
6323 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6324 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6325 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6326 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6327 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6328 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6329 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6330 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6331 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6332 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6333 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6334 final double[] bias1 = calibrator.getInitialBias();
6335 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6336 final double[] bias2 = new double[3];
6337 calibrator.getInitialBias(bias2);
6338 assertArrayEquals(bias1, bias2, 0.0);
6339 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6340 assertEquals(b1, ba);
6341 final Matrix b2 = new Matrix(3, 1);
6342 calibrator.getInitialBiasAsMatrix(b2);
6343 assertEquals(b1, b2);
6344 final Matrix ma1 = calibrator.getInitialMa();
6345 assertEquals(ma1, new Matrix(3, 3));
6346 final Matrix ma2 = new Matrix(3, 3);
6347 calibrator.getInitialMa(ma2);
6348 assertEquals(ma1, ma2);
6349 assertSame(calibrator.getMeasurements(), measurements);
6350 assertFalse(calibrator.isCommonAxisUsed());
6351 assertSame(calibrator.getListener(), this);
6352 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6353 assertFalse(calibrator.isReady());
6354 assertFalse(calibrator.isRunning());
6355 assertNull(calibrator.getEstimatedBiases());
6356 assertFalse(calibrator.getEstimatedBiases(null));
6357 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6358 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6359 assertNull(calibrator.getEstimatedBiasFx());
6360 assertNull(calibrator.getEstimatedBiasFy());
6361 assertNull(calibrator.getEstimatedBiasFz());
6362 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6363 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6364 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6365 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6366 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6367 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6368 assertNull(calibrator.getEstimatedMa());
6369 assertNull(calibrator.getEstimatedSx());
6370 assertNull(calibrator.getEstimatedSy());
6371 assertNull(calibrator.getEstimatedSz());
6372 assertNull(calibrator.getEstimatedMxy());
6373 assertNull(calibrator.getEstimatedMxz());
6374 assertNull(calibrator.getEstimatedMyx());
6375 assertNull(calibrator.getEstimatedMyz());
6376 assertNull(calibrator.getEstimatedMzx());
6377 assertNull(calibrator.getEstimatedMzy());
6378 assertNull(calibrator.getEstimatedCovariance());
6379 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6380 assertNull(calibrator.getGroundTruthGravityNorm());
6381 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6382 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6383
6384
6385 calibrator = null;
6386 try {
6387 calibrator = new KnownGravityNormAccelerometerCalibrator(
6388 measurements, new double[1], this);
6389 fail("IllegalArgumentException expected but not thrown");
6390 } catch (final IllegalArgumentException ignore) {
6391 }
6392 assertNull(calibrator);
6393 }
6394
6395 @Test
6396 public void testConstructor59() throws WrongSizeException {
6397 final Matrix ba = generateBa();
6398 final double[] bias = ba.getBuffer();
6399 final double biasX = ba.getElementAtIndex(0);
6400 final double biasY = ba.getElementAtIndex(1);
6401 final double biasZ = ba.getElementAtIndex(2);
6402
6403 KnownGravityNormAccelerometerCalibrator calibrator =
6404 new KnownGravityNormAccelerometerCalibrator(
6405 true, bias);
6406
6407
6408 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6409 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6410 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6411 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6412 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6413 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6414 final Acceleration bx2 = new Acceleration(0.0,
6415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6416 calibrator.getInitialBiasXAsAcceleration(bx2);
6417 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6418 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6419 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6420 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6421 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6422 final Acceleration by2 = new Acceleration(0.0,
6423 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6424 calibrator.getInitialBiasYAsAcceleration(by2);
6425 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6426 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6427 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6428 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6429 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6430 final Acceleration bz2 = new Acceleration(0.0,
6431 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6432 calibrator.getInitialBiasZAsAcceleration(bz2);
6433 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6434 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6435 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6436 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6437 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6438 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6439 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6440 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6441 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6442 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6443 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6444 final double[] bias1 = calibrator.getInitialBias();
6445 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6446 final double[] bias2 = new double[3];
6447 calibrator.getInitialBias(bias2);
6448 assertArrayEquals(bias1, bias2, 0.0);
6449 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6450 assertEquals(b1, ba);
6451 final Matrix b2 = new Matrix(3, 1);
6452 calibrator.getInitialBiasAsMatrix(b2);
6453 assertEquals(b1, b2);
6454 final Matrix ma1 = calibrator.getInitialMa();
6455 assertEquals(ma1, new Matrix(3, 3));
6456 final Matrix ma2 = new Matrix(3, 3);
6457 calibrator.getInitialMa(ma2);
6458 assertEquals(ma1, ma2);
6459 assertNull(calibrator.getMeasurements());
6460 assertTrue(calibrator.isCommonAxisUsed());
6461 assertNull(calibrator.getListener());
6462 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6463 assertFalse(calibrator.isReady());
6464 assertFalse(calibrator.isRunning());
6465 assertNull(calibrator.getEstimatedBiases());
6466 assertFalse(calibrator.getEstimatedBiases(null));
6467 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6468 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6469 assertNull(calibrator.getEstimatedBiasFx());
6470 assertNull(calibrator.getEstimatedBiasFy());
6471 assertNull(calibrator.getEstimatedBiasFz());
6472 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6473 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6474 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6475 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6476 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6477 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6478 assertNull(calibrator.getEstimatedMa());
6479 assertNull(calibrator.getEstimatedSx());
6480 assertNull(calibrator.getEstimatedSy());
6481 assertNull(calibrator.getEstimatedSz());
6482 assertNull(calibrator.getEstimatedMxy());
6483 assertNull(calibrator.getEstimatedMxz());
6484 assertNull(calibrator.getEstimatedMyx());
6485 assertNull(calibrator.getEstimatedMyz());
6486 assertNull(calibrator.getEstimatedMzx());
6487 assertNull(calibrator.getEstimatedMzy());
6488 assertNull(calibrator.getEstimatedCovariance());
6489 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6490 assertNull(calibrator.getGroundTruthGravityNorm());
6491 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6492 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6493
6494
6495 calibrator = null;
6496 try {
6497 calibrator = new KnownGravityNormAccelerometerCalibrator(
6498 true, new double[1]);
6499 fail("IllegalArgumentException expected but not thrown");
6500 } catch (final IllegalArgumentException ignore) {
6501 }
6502 assertNull(calibrator);
6503 }
6504
6505 @Test
6506 public void testConstructor60() throws WrongSizeException {
6507 final Matrix ba = generateBa();
6508 final double[] bias = ba.getBuffer();
6509 final double biasX = ba.getElementAtIndex(0);
6510 final double biasY = ba.getElementAtIndex(1);
6511 final double biasZ = ba.getElementAtIndex(2);
6512
6513 KnownGravityNormAccelerometerCalibrator calibrator =
6514 new KnownGravityNormAccelerometerCalibrator(
6515 true, bias, this);
6516
6517
6518 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6519 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6520 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6521 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6522 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6523 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6524 final Acceleration bx2 = new Acceleration(0.0,
6525 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6526 calibrator.getInitialBiasXAsAcceleration(bx2);
6527 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6528 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6529 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6530 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6531 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6532 final Acceleration by2 = new Acceleration(0.0,
6533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6534 calibrator.getInitialBiasYAsAcceleration(by2);
6535 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6536 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6537 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6538 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6539 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6540 final Acceleration bz2 = new Acceleration(0.0,
6541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6542 calibrator.getInitialBiasZAsAcceleration(bz2);
6543 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6544 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6545 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6546 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6547 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6548 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6549 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6550 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6551 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6552 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6553 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6554 final double[] bias1 = calibrator.getInitialBias();
6555 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6556 final double[] bias2 = new double[3];
6557 calibrator.getInitialBias(bias2);
6558 assertArrayEquals(bias1, bias2, 0.0);
6559 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6560 assertEquals(b1, ba);
6561 final Matrix b2 = new Matrix(3, 1);
6562 calibrator.getInitialBiasAsMatrix(b2);
6563 assertEquals(b1, b2);
6564 final Matrix ma1 = calibrator.getInitialMa();
6565 assertEquals(ma1, new Matrix(3, 3));
6566 final Matrix ma2 = new Matrix(3, 3);
6567 calibrator.getInitialMa(ma2);
6568 assertEquals(ma1, ma2);
6569 assertNull(calibrator.getMeasurements());
6570 assertTrue(calibrator.isCommonAxisUsed());
6571 assertSame(calibrator.getListener(), this);
6572 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6573 assertFalse(calibrator.isReady());
6574 assertFalse(calibrator.isRunning());
6575 assertNull(calibrator.getEstimatedBiases());
6576 assertFalse(calibrator.getEstimatedBiases(null));
6577 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6578 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6579 assertNull(calibrator.getEstimatedBiasFx());
6580 assertNull(calibrator.getEstimatedBiasFy());
6581 assertNull(calibrator.getEstimatedBiasFz());
6582 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6583 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6584 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6585 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6586 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6587 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6588 assertNull(calibrator.getEstimatedMa());
6589 assertNull(calibrator.getEstimatedSx());
6590 assertNull(calibrator.getEstimatedSy());
6591 assertNull(calibrator.getEstimatedSz());
6592 assertNull(calibrator.getEstimatedMxy());
6593 assertNull(calibrator.getEstimatedMxz());
6594 assertNull(calibrator.getEstimatedMyx());
6595 assertNull(calibrator.getEstimatedMyz());
6596 assertNull(calibrator.getEstimatedMzx());
6597 assertNull(calibrator.getEstimatedMzy());
6598 assertNull(calibrator.getEstimatedCovariance());
6599 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6600 assertNull(calibrator.getGroundTruthGravityNorm());
6601 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6602 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6603
6604
6605 calibrator = null;
6606 try {
6607 calibrator = new KnownGravityNormAccelerometerCalibrator(
6608 true, new double[1], this);
6609 fail("IllegalArgumentException expected but not thrown");
6610 } catch (final IllegalArgumentException ignore) {
6611 }
6612 assertNull(calibrator);
6613 }
6614
6615 @Test
6616 public void testConstructor61() throws WrongSizeException {
6617 final Collection<StandardDeviationBodyKinematics> measurements =
6618 Collections.emptyList();
6619
6620 final Matrix ba = generateBa();
6621 final double[] bias = ba.getBuffer();
6622 final double biasX = ba.getElementAtIndex(0);
6623 final double biasY = ba.getElementAtIndex(1);
6624 final double biasZ = ba.getElementAtIndex(2);
6625
6626 KnownGravityNormAccelerometerCalibrator calibrator =
6627 new KnownGravityNormAccelerometerCalibrator(measurements,
6628 true, bias);
6629
6630
6631 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6632 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6633 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6634 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6635 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6636 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6637 final Acceleration bx2 = new Acceleration(0.0,
6638 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6639 calibrator.getInitialBiasXAsAcceleration(bx2);
6640 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6641 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6642 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6643 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6644 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6645 final Acceleration by2 = new Acceleration(0.0,
6646 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6647 calibrator.getInitialBiasYAsAcceleration(by2);
6648 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6649 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6650 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6651 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6652 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6653 final Acceleration bz2 = new Acceleration(0.0,
6654 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6655 calibrator.getInitialBiasZAsAcceleration(bz2);
6656 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6657 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6658 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6659 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6660 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6661 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6662 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6663 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6664 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6665 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6666 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6667 final double[] bias1 = calibrator.getInitialBias();
6668 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6669 final double[] bias2 = new double[3];
6670 calibrator.getInitialBias(bias2);
6671 assertArrayEquals(bias1, bias2, 0.0);
6672 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6673 assertEquals(b1, ba);
6674 final Matrix b2 = new Matrix(3, 1);
6675 calibrator.getInitialBiasAsMatrix(b2);
6676 assertEquals(b1, b2);
6677 final Matrix ma1 = calibrator.getInitialMa();
6678 assertEquals(ma1, new Matrix(3, 3));
6679 final Matrix ma2 = new Matrix(3, 3);
6680 calibrator.getInitialMa(ma2);
6681 assertEquals(ma1, ma2);
6682 assertSame(calibrator.getMeasurements(), measurements);
6683 assertTrue(calibrator.isCommonAxisUsed());
6684 assertNull(calibrator.getListener());
6685 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6686 assertFalse(calibrator.isReady());
6687 assertFalse(calibrator.isRunning());
6688 assertNull(calibrator.getEstimatedBiases());
6689 assertFalse(calibrator.getEstimatedBiases(null));
6690 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6691 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6692 assertNull(calibrator.getEstimatedBiasFx());
6693 assertNull(calibrator.getEstimatedBiasFy());
6694 assertNull(calibrator.getEstimatedBiasFz());
6695 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6696 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6697 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6698 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6699 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6700 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6701 assertNull(calibrator.getEstimatedMa());
6702 assertNull(calibrator.getEstimatedSx());
6703 assertNull(calibrator.getEstimatedSy());
6704 assertNull(calibrator.getEstimatedSz());
6705 assertNull(calibrator.getEstimatedMxy());
6706 assertNull(calibrator.getEstimatedMxz());
6707 assertNull(calibrator.getEstimatedMyx());
6708 assertNull(calibrator.getEstimatedMyz());
6709 assertNull(calibrator.getEstimatedMzx());
6710 assertNull(calibrator.getEstimatedMzy());
6711 assertNull(calibrator.getEstimatedCovariance());
6712 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6713 assertNull(calibrator.getGroundTruthGravityNorm());
6714 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6715 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6716
6717
6718 calibrator = null;
6719 try {
6720 calibrator = new KnownGravityNormAccelerometerCalibrator(
6721 measurements, true, new double[1]);
6722 fail("IllegalArgumentException expected but not thrown");
6723 } catch (final IllegalArgumentException ignore) {
6724 }
6725 assertNull(calibrator);
6726 }
6727
6728 @Test
6729 public void testConstructor62() throws WrongSizeException {
6730 final Collection<StandardDeviationBodyKinematics> measurements =
6731 Collections.emptyList();
6732
6733 final Matrix ba = generateBa();
6734 final double[] bias = ba.getBuffer();
6735 final double biasX = ba.getElementAtIndex(0);
6736 final double biasY = ba.getElementAtIndex(1);
6737 final double biasZ = ba.getElementAtIndex(2);
6738
6739 KnownGravityNormAccelerometerCalibrator calibrator =
6740 new KnownGravityNormAccelerometerCalibrator(measurements,
6741 true, bias, this);
6742
6743
6744 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6745 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6746 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6747 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6748 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6749 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6750 final Acceleration bx2 = new Acceleration(0.0,
6751 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6752 calibrator.getInitialBiasXAsAcceleration(bx2);
6753 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6754 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6755 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6756 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6757 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6758 final Acceleration by2 = new Acceleration(0.0,
6759 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6760 calibrator.getInitialBiasYAsAcceleration(by2);
6761 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6762 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6763 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6764 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6765 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6766 final Acceleration bz2 = new Acceleration(0.0,
6767 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6768 calibrator.getInitialBiasZAsAcceleration(bz2);
6769 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6770 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6771 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6772 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6773 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6774 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6775 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6776 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6777 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6778 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6779 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6780 final double[] bias1 = calibrator.getInitialBias();
6781 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6782 final double[] bias2 = new double[3];
6783 calibrator.getInitialBias(bias2);
6784 assertArrayEquals(bias1, bias2, 0.0);
6785 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6786 assertEquals(b1, ba);
6787 final Matrix b2 = new Matrix(3, 1);
6788 calibrator.getInitialBiasAsMatrix(b2);
6789 assertEquals(b1, b2);
6790 final Matrix ma1 = calibrator.getInitialMa();
6791 assertEquals(ma1, new Matrix(3, 3));
6792 final Matrix ma2 = new Matrix(3, 3);
6793 calibrator.getInitialMa(ma2);
6794 assertEquals(ma1, ma2);
6795 assertSame(calibrator.getMeasurements(), measurements);
6796 assertTrue(calibrator.isCommonAxisUsed());
6797 assertSame(calibrator.getListener(), this);
6798 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6799 assertFalse(calibrator.isReady());
6800 assertFalse(calibrator.isRunning());
6801 assertNull(calibrator.getEstimatedBiases());
6802 assertFalse(calibrator.getEstimatedBiases(null));
6803 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6804 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6805 assertNull(calibrator.getEstimatedBiasFx());
6806 assertNull(calibrator.getEstimatedBiasFy());
6807 assertNull(calibrator.getEstimatedBiasFz());
6808 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6809 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6810 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6811 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6812 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6813 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6814 assertNull(calibrator.getEstimatedMa());
6815 assertNull(calibrator.getEstimatedSx());
6816 assertNull(calibrator.getEstimatedSy());
6817 assertNull(calibrator.getEstimatedSz());
6818 assertNull(calibrator.getEstimatedMxy());
6819 assertNull(calibrator.getEstimatedMxz());
6820 assertNull(calibrator.getEstimatedMyx());
6821 assertNull(calibrator.getEstimatedMyz());
6822 assertNull(calibrator.getEstimatedMzx());
6823 assertNull(calibrator.getEstimatedMzy());
6824 assertNull(calibrator.getEstimatedCovariance());
6825 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6826 assertNull(calibrator.getGroundTruthGravityNorm());
6827 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6828 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6829
6830
6831 calibrator = null;
6832 try {
6833 calibrator = new KnownGravityNormAccelerometerCalibrator(
6834 measurements, true, new double[1],
6835 this);
6836 fail("IllegalArgumentException expected but not thrown");
6837 } catch (final IllegalArgumentException ignore) {
6838 }
6839 assertNull(calibrator);
6840 }
6841
6842 @Test
6843 public void testConstructor63() throws WrongSizeException {
6844 final Matrix ba = generateBa();
6845 final double biasX = ba.getElementAtIndex(0);
6846 final double biasY = ba.getElementAtIndex(1);
6847 final double biasZ = ba.getElementAtIndex(2);
6848
6849 KnownGravityNormAccelerometerCalibrator calibrator =
6850 new KnownGravityNormAccelerometerCalibrator(ba);
6851
6852
6853 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6854 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6855 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6856 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6857 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6858 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6859 final Acceleration bx2 = new Acceleration(0.0,
6860 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6861 calibrator.getInitialBiasXAsAcceleration(bx2);
6862 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6863 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6864 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6865 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6866 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6867 final Acceleration by2 = new Acceleration(0.0,
6868 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6869 calibrator.getInitialBiasYAsAcceleration(by2);
6870 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6871 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6872 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6873 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6874 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6875 final Acceleration bz2 = new Acceleration(0.0,
6876 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6877 calibrator.getInitialBiasZAsAcceleration(bz2);
6878 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6879 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6880 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6881 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6882 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6883 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6884 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6885 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6886 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6887 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6888 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6889 final double[] bias1 = calibrator.getInitialBias();
6890 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6891 final double[] bias2 = new double[3];
6892 calibrator.getInitialBias(bias2);
6893 assertArrayEquals(bias1, bias2, 0.0);
6894 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6895 assertEquals(b1, ba);
6896 final Matrix b2 = new Matrix(3, 1);
6897 calibrator.getInitialBiasAsMatrix(b2);
6898 assertEquals(b1, b2);
6899 final Matrix ma1 = calibrator.getInitialMa();
6900 assertEquals(ma1, new Matrix(3, 3));
6901 final Matrix ma2 = new Matrix(3, 3);
6902 calibrator.getInitialMa(ma2);
6903 assertEquals(ma1, ma2);
6904 assertNull(calibrator.getMeasurements());
6905 assertFalse(calibrator.isCommonAxisUsed());
6906 assertNull(calibrator.getListener());
6907 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6908 assertFalse(calibrator.isReady());
6909 assertFalse(calibrator.isRunning());
6910 assertNull(calibrator.getEstimatedBiases());
6911 assertFalse(calibrator.getEstimatedBiases(null));
6912 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6913 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6914 assertNull(calibrator.getEstimatedBiasFx());
6915 assertNull(calibrator.getEstimatedBiasFy());
6916 assertNull(calibrator.getEstimatedBiasFz());
6917 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6918 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6919 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6920 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6921 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6922 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6923 assertNull(calibrator.getEstimatedMa());
6924 assertNull(calibrator.getEstimatedSx());
6925 assertNull(calibrator.getEstimatedSy());
6926 assertNull(calibrator.getEstimatedSz());
6927 assertNull(calibrator.getEstimatedMxy());
6928 assertNull(calibrator.getEstimatedMxz());
6929 assertNull(calibrator.getEstimatedMyx());
6930 assertNull(calibrator.getEstimatedMyz());
6931 assertNull(calibrator.getEstimatedMzx());
6932 assertNull(calibrator.getEstimatedMzy());
6933 assertNull(calibrator.getEstimatedCovariance());
6934 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6935 assertNull(calibrator.getGroundTruthGravityNorm());
6936 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6937 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6938
6939
6940 calibrator = null;
6941 try {
6942 calibrator = new KnownGravityNormAccelerometerCalibrator(
6943 new Matrix(1, 1));
6944 fail("IllegalArgumentException expected but not thrown");
6945 } catch (final IllegalArgumentException ignore) {
6946 }
6947 try {
6948 calibrator = new KnownGravityNormAccelerometerCalibrator(
6949 new Matrix(1, 3));
6950 fail("IllegalArgumentException expected but not thrown");
6951 } catch (final IllegalArgumentException ignore) {
6952 }
6953 assertNull(calibrator);
6954 }
6955
6956 @Test
6957 public void testConstructor64() throws WrongSizeException {
6958 final Matrix ba = generateBa();
6959 final double biasX = ba.getElementAtIndex(0);
6960 final double biasY = ba.getElementAtIndex(1);
6961 final double biasZ = ba.getElementAtIndex(2);
6962
6963 KnownGravityNormAccelerometerCalibrator calibrator =
6964 new KnownGravityNormAccelerometerCalibrator(
6965 ba, this);
6966
6967
6968 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6969 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6970 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6971 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6972 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6973 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6974 final Acceleration bx2 = new Acceleration(0.0,
6975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6976 calibrator.getInitialBiasXAsAcceleration(bx2);
6977 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6978 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6979 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6980 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6981 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6982 final Acceleration by2 = new Acceleration(0.0,
6983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6984 calibrator.getInitialBiasYAsAcceleration(by2);
6985 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6986 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6987 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6988 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6989 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6990 final Acceleration bz2 = new Acceleration(0.0,
6991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6992 calibrator.getInitialBiasZAsAcceleration(bz2);
6993 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6994 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6995 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6996 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6997 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6998 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6999 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7000 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7001 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7002 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7003 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7004 final double[] bias1 = calibrator.getInitialBias();
7005 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7006 final double[] bias2 = new double[3];
7007 calibrator.getInitialBias(bias2);
7008 assertArrayEquals(bias1, bias2, 0.0);
7009 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7010 assertEquals(b1, ba);
7011 final Matrix b2 = new Matrix(3, 1);
7012 calibrator.getInitialBiasAsMatrix(b2);
7013 assertEquals(b1, b2);
7014 final Matrix ma1 = calibrator.getInitialMa();
7015 assertEquals(ma1, new Matrix(3, 3));
7016 final Matrix ma2 = new Matrix(3, 3);
7017 calibrator.getInitialMa(ma2);
7018 assertEquals(ma1, ma2);
7019 assertNull(calibrator.getMeasurements());
7020 assertFalse(calibrator.isCommonAxisUsed());
7021 assertSame(calibrator.getListener(), this);
7022 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7023 assertFalse(calibrator.isReady());
7024 assertFalse(calibrator.isRunning());
7025 assertNull(calibrator.getEstimatedBiases());
7026 assertFalse(calibrator.getEstimatedBiases(null));
7027 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7028 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7029 assertNull(calibrator.getEstimatedBiasFx());
7030 assertNull(calibrator.getEstimatedBiasFy());
7031 assertNull(calibrator.getEstimatedBiasFz());
7032 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7033 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7034 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7035 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7036 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7037 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7038 assertNull(calibrator.getEstimatedMa());
7039 assertNull(calibrator.getEstimatedSx());
7040 assertNull(calibrator.getEstimatedSy());
7041 assertNull(calibrator.getEstimatedSz());
7042 assertNull(calibrator.getEstimatedMxy());
7043 assertNull(calibrator.getEstimatedMxz());
7044 assertNull(calibrator.getEstimatedMyx());
7045 assertNull(calibrator.getEstimatedMyz());
7046 assertNull(calibrator.getEstimatedMzx());
7047 assertNull(calibrator.getEstimatedMzy());
7048 assertNull(calibrator.getEstimatedCovariance());
7049 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7050 assertNull(calibrator.getGroundTruthGravityNorm());
7051 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7052 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7053
7054
7055 calibrator = null;
7056 try {
7057 calibrator = new KnownGravityNormAccelerometerCalibrator(
7058 new Matrix(1, 1));
7059 fail("IllegalArgumentException expected but not thrown");
7060 } catch (final IllegalArgumentException ignore) {
7061 }
7062 try {
7063 calibrator = new KnownGravityNormAccelerometerCalibrator(
7064 new Matrix(1, 3));
7065 fail("IllegalArgumentException expected but not thrown");
7066 } catch (final IllegalArgumentException ignore) {
7067 }
7068 assertNull(calibrator);
7069 }
7070
7071 @Test
7072 public void testConstructor65() throws WrongSizeException {
7073 final Collection<StandardDeviationBodyKinematics> measurements =
7074 Collections.emptyList();
7075
7076 final Matrix ba = generateBa();
7077 final double biasX = ba.getElementAtIndex(0);
7078 final double biasY = ba.getElementAtIndex(1);
7079 final double biasZ = ba.getElementAtIndex(2);
7080
7081 KnownGravityNormAccelerometerCalibrator calibrator =
7082 new KnownGravityNormAccelerometerCalibrator(
7083 measurements, ba);
7084
7085
7086 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7087 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7088 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7089 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7090 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7091 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7092 final Acceleration bx2 = new Acceleration(0.0,
7093 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7094 calibrator.getInitialBiasXAsAcceleration(bx2);
7095 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7096 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7097 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7098 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7099 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7100 final Acceleration by2 = new Acceleration(0.0,
7101 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7102 calibrator.getInitialBiasYAsAcceleration(by2);
7103 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7104 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7105 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7106 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7107 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7108 final Acceleration bz2 = new Acceleration(0.0,
7109 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7110 calibrator.getInitialBiasZAsAcceleration(bz2);
7111 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7112 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7113 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7114 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7115 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7116 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7117 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7118 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7119 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7120 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7121 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7122 final double[] bias1 = calibrator.getInitialBias();
7123 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7124 final double[] bias2 = new double[3];
7125 calibrator.getInitialBias(bias2);
7126 assertArrayEquals(bias1, bias2, 0.0);
7127 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7128 assertEquals(b1, ba);
7129 final Matrix b2 = new Matrix(3, 1);
7130 calibrator.getInitialBiasAsMatrix(b2);
7131 assertEquals(b1, b2);
7132 final Matrix ma1 = calibrator.getInitialMa();
7133 assertEquals(ma1, new Matrix(3, 3));
7134 final Matrix ma2 = new Matrix(3, 3);
7135 calibrator.getInitialMa(ma2);
7136 assertEquals(ma1, ma2);
7137 assertSame(calibrator.getMeasurements(), measurements);
7138 assertFalse(calibrator.isCommonAxisUsed());
7139 assertNull(calibrator.getListener());
7140 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7141 assertFalse(calibrator.isReady());
7142 assertFalse(calibrator.isRunning());
7143 assertNull(calibrator.getEstimatedBiases());
7144 assertFalse(calibrator.getEstimatedBiases(null));
7145 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7146 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7147 assertNull(calibrator.getEstimatedBiasFx());
7148 assertNull(calibrator.getEstimatedBiasFy());
7149 assertNull(calibrator.getEstimatedBiasFz());
7150 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7151 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7152 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7153 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7154 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7155 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7156 assertNull(calibrator.getEstimatedMa());
7157 assertNull(calibrator.getEstimatedSx());
7158 assertNull(calibrator.getEstimatedSy());
7159 assertNull(calibrator.getEstimatedSz());
7160 assertNull(calibrator.getEstimatedMxy());
7161 assertNull(calibrator.getEstimatedMxz());
7162 assertNull(calibrator.getEstimatedMyx());
7163 assertNull(calibrator.getEstimatedMyz());
7164 assertNull(calibrator.getEstimatedMzx());
7165 assertNull(calibrator.getEstimatedMzy());
7166 assertNull(calibrator.getEstimatedCovariance());
7167 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7168 assertNull(calibrator.getGroundTruthGravityNorm());
7169 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7170 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7171
7172
7173 calibrator = null;
7174 try {
7175 calibrator = new KnownGravityNormAccelerometerCalibrator(measurements,
7176 new Matrix(1, 1));
7177 fail("IllegalArgumentException expected but not thrown");
7178 } catch (final IllegalArgumentException ignore) {
7179 }
7180 try {
7181 calibrator = new KnownGravityNormAccelerometerCalibrator(measurements,
7182 new Matrix(1, 3));
7183 fail("IllegalArgumentException expected but not thrown");
7184 } catch (final IllegalArgumentException ignore) {
7185 }
7186 assertNull(calibrator);
7187 }
7188
7189 @Test
7190 public void testConstructor66() throws WrongSizeException {
7191 final Collection<StandardDeviationBodyKinematics> measurements =
7192 Collections.emptyList();
7193
7194 final Matrix ba = generateBa();
7195 final double biasX = ba.getElementAtIndex(0);
7196 final double biasY = ba.getElementAtIndex(1);
7197 final double biasZ = ba.getElementAtIndex(2);
7198
7199 KnownGravityNormAccelerometerCalibrator calibrator =
7200 new KnownGravityNormAccelerometerCalibrator(measurements, ba,
7201 this);
7202
7203
7204 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7205 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7206 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7207 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7208 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7209 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7210 final Acceleration bx2 = new Acceleration(0.0,
7211 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7212 calibrator.getInitialBiasXAsAcceleration(bx2);
7213 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7214 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7215 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7216 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7217 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7218 final Acceleration by2 = new Acceleration(0.0,
7219 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7220 calibrator.getInitialBiasYAsAcceleration(by2);
7221 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7222 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7223 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7224 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7225 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7226 final Acceleration bz2 = new Acceleration(0.0,
7227 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7228 calibrator.getInitialBiasZAsAcceleration(bz2);
7229 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7230 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7231 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7232 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7233 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7234 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7235 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7236 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7237 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7238 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7239 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7240 final double[] bias1 = calibrator.getInitialBias();
7241 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7242 final double[] bias2 = new double[3];
7243 calibrator.getInitialBias(bias2);
7244 assertArrayEquals(bias1, bias2, 0.0);
7245 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7246 assertEquals(b1, ba);
7247 final Matrix b2 = new Matrix(3, 1);
7248 calibrator.getInitialBiasAsMatrix(b2);
7249 assertEquals(b1, b2);
7250 final Matrix ma1 = calibrator.getInitialMa();
7251 assertEquals(ma1, new Matrix(3, 3));
7252 final Matrix ma2 = new Matrix(3, 3);
7253 calibrator.getInitialMa(ma2);
7254 assertEquals(ma1, ma2);
7255 assertSame(calibrator.getMeasurements(), measurements);
7256 assertFalse(calibrator.isCommonAxisUsed());
7257 assertSame(calibrator.getListener(), this);
7258 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7259 assertFalse(calibrator.isReady());
7260 assertFalse(calibrator.isRunning());
7261 assertNull(calibrator.getEstimatedBiases());
7262 assertFalse(calibrator.getEstimatedBiases(null));
7263 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7264 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7265 assertNull(calibrator.getEstimatedBiasFx());
7266 assertNull(calibrator.getEstimatedBiasFy());
7267 assertNull(calibrator.getEstimatedBiasFz());
7268 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7269 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7270 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7271 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7272 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7273 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7274 assertNull(calibrator.getEstimatedMa());
7275 assertNull(calibrator.getEstimatedSx());
7276 assertNull(calibrator.getEstimatedSy());
7277 assertNull(calibrator.getEstimatedSz());
7278 assertNull(calibrator.getEstimatedMxy());
7279 assertNull(calibrator.getEstimatedMxz());
7280 assertNull(calibrator.getEstimatedMyx());
7281 assertNull(calibrator.getEstimatedMyz());
7282 assertNull(calibrator.getEstimatedMzx());
7283 assertNull(calibrator.getEstimatedMzy());
7284 assertNull(calibrator.getEstimatedCovariance());
7285 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7286 assertNull(calibrator.getGroundTruthGravityNorm());
7287 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7288 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7289
7290
7291 calibrator = null;
7292 try {
7293 calibrator = new KnownGravityNormAccelerometerCalibrator(
7294 measurements, new Matrix(1, 1),
7295 this);
7296 fail("IllegalArgumentException expected but not thrown");
7297 } catch (final IllegalArgumentException ignore) {
7298 }
7299 try {
7300 calibrator = new KnownGravityNormAccelerometerCalibrator(
7301 measurements, new Matrix(1, 3),
7302 this);
7303 fail("IllegalArgumentException expected but not thrown");
7304 } catch (final IllegalArgumentException ignore) {
7305 }
7306 assertNull(calibrator);
7307 }
7308
7309 @Test
7310 public void testConstructor67() throws WrongSizeException {
7311 final Matrix ba = generateBa();
7312 final double biasX = ba.getElementAtIndex(0);
7313 final double biasY = ba.getElementAtIndex(1);
7314 final double biasZ = ba.getElementAtIndex(2);
7315
7316 KnownGravityNormAccelerometerCalibrator calibrator =
7317 new KnownGravityNormAccelerometerCalibrator(
7318 true, ba);
7319
7320
7321 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7322 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7323 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7324 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7325 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7326 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7327 final Acceleration bx2 = new Acceleration(0.0,
7328 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7329 calibrator.getInitialBiasXAsAcceleration(bx2);
7330 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7331 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7332 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7333 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7334 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7335 final Acceleration by2 = new Acceleration(0.0,
7336 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7337 calibrator.getInitialBiasYAsAcceleration(by2);
7338 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7339 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7340 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7341 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7342 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7343 final Acceleration bz2 = new Acceleration(0.0,
7344 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7345 calibrator.getInitialBiasZAsAcceleration(bz2);
7346 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7347 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7348 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7349 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7350 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7351 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7352 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7353 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7354 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7355 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7356 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7357 final double[] bias1 = calibrator.getInitialBias();
7358 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7359 final double[] bias2 = new double[3];
7360 calibrator.getInitialBias(bias2);
7361 assertArrayEquals(bias1, bias2, 0.0);
7362 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7363 assertEquals(b1, ba);
7364 final Matrix b2 = new Matrix(3, 1);
7365 calibrator.getInitialBiasAsMatrix(b2);
7366 assertEquals(b1, b2);
7367 final Matrix ma1 = calibrator.getInitialMa();
7368 assertEquals(ma1, new Matrix(3, 3));
7369 final Matrix ma2 = new Matrix(3, 3);
7370 calibrator.getInitialMa(ma2);
7371 assertEquals(ma1, ma2);
7372 assertNull(calibrator.getMeasurements());
7373 assertTrue(calibrator.isCommonAxisUsed());
7374 assertNull(calibrator.getListener());
7375 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7376 assertFalse(calibrator.isReady());
7377 assertFalse(calibrator.isRunning());
7378 assertNull(calibrator.getEstimatedBiases());
7379 assertFalse(calibrator.getEstimatedBiases(null));
7380 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7381 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7382 assertNull(calibrator.getEstimatedBiasFx());
7383 assertNull(calibrator.getEstimatedBiasFy());
7384 assertNull(calibrator.getEstimatedBiasFz());
7385 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7386 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7387 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7388 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7389 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7390 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7391 assertNull(calibrator.getEstimatedMa());
7392 assertNull(calibrator.getEstimatedSx());
7393 assertNull(calibrator.getEstimatedSy());
7394 assertNull(calibrator.getEstimatedSz());
7395 assertNull(calibrator.getEstimatedMxy());
7396 assertNull(calibrator.getEstimatedMxz());
7397 assertNull(calibrator.getEstimatedMyx());
7398 assertNull(calibrator.getEstimatedMyz());
7399 assertNull(calibrator.getEstimatedMzx());
7400 assertNull(calibrator.getEstimatedMzy());
7401 assertNull(calibrator.getEstimatedCovariance());
7402 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7403 assertNull(calibrator.getGroundTruthGravityNorm());
7404 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7405 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7406
7407
7408 calibrator = null;
7409 try {
7410 calibrator = new KnownGravityNormAccelerometerCalibrator(
7411 true, new Matrix(1, 1));
7412 fail("IllegalArgumentException expected but not thrown");
7413 } catch (final IllegalArgumentException ignore) {
7414 }
7415 try {
7416 calibrator = new KnownGravityNormAccelerometerCalibrator(
7417 true, new Matrix(1, 3));
7418 fail("IllegalArgumentException expected but not thrown");
7419 } catch (final IllegalArgumentException ignore) {
7420 }
7421 assertNull(calibrator);
7422 }
7423
7424 @Test
7425 public void testConstructor68() throws WrongSizeException {
7426 final Matrix ba = generateBa();
7427 final double biasX = ba.getElementAtIndex(0);
7428 final double biasY = ba.getElementAtIndex(1);
7429 final double biasZ = ba.getElementAtIndex(2);
7430
7431 KnownGravityNormAccelerometerCalibrator calibrator =
7432 new KnownGravityNormAccelerometerCalibrator(
7433 true, ba, this);
7434
7435
7436 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7437 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7438 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7439 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7440 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7441 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7442 final Acceleration bx2 = new Acceleration(0.0,
7443 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7444 calibrator.getInitialBiasXAsAcceleration(bx2);
7445 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7446 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7447 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7448 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7449 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7450 final Acceleration by2 = new Acceleration(0.0,
7451 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7452 calibrator.getInitialBiasYAsAcceleration(by2);
7453 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7454 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7455 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7456 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7457 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7458 final Acceleration bz2 = new Acceleration(0.0,
7459 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7460 calibrator.getInitialBiasZAsAcceleration(bz2);
7461 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7462 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7463 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7464 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7465 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7466 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7467 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7468 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7469 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7470 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7471 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7472 final double[] bias1 = calibrator.getInitialBias();
7473 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7474 final double[] bias2 = new double[3];
7475 calibrator.getInitialBias(bias2);
7476 assertArrayEquals(bias1, bias2, 0.0);
7477 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7478 assertEquals(b1, ba);
7479 final Matrix b2 = new Matrix(3, 1);
7480 calibrator.getInitialBiasAsMatrix(b2);
7481 assertEquals(b1, b2);
7482 final Matrix ma1 = calibrator.getInitialMa();
7483 assertEquals(ma1, new Matrix(3, 3));
7484 final Matrix ma2 = new Matrix(3, 3);
7485 calibrator.getInitialMa(ma2);
7486 assertEquals(ma1, ma2);
7487 assertNull(calibrator.getMeasurements());
7488 assertTrue(calibrator.isCommonAxisUsed());
7489 assertSame(calibrator.getListener(), this);
7490 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7491 assertFalse(calibrator.isReady());
7492 assertFalse(calibrator.isRunning());
7493 assertNull(calibrator.getEstimatedBiases());
7494 assertFalse(calibrator.getEstimatedBiases(null));
7495 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7496 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7497 assertNull(calibrator.getEstimatedBiasFx());
7498 assertNull(calibrator.getEstimatedBiasFy());
7499 assertNull(calibrator.getEstimatedBiasFz());
7500 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7501 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7502 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7503 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7504 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7505 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7506 assertNull(calibrator.getEstimatedMa());
7507 assertNull(calibrator.getEstimatedSx());
7508 assertNull(calibrator.getEstimatedSy());
7509 assertNull(calibrator.getEstimatedSz());
7510 assertNull(calibrator.getEstimatedMxy());
7511 assertNull(calibrator.getEstimatedMxz());
7512 assertNull(calibrator.getEstimatedMyx());
7513 assertNull(calibrator.getEstimatedMyz());
7514 assertNull(calibrator.getEstimatedMzx());
7515 assertNull(calibrator.getEstimatedMzy());
7516 assertNull(calibrator.getEstimatedCovariance());
7517 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7518 assertNull(calibrator.getGroundTruthGravityNorm());
7519 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7520 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7521
7522
7523 calibrator = null;
7524 try {
7525 calibrator = new KnownGravityNormAccelerometerCalibrator(
7526 true, new Matrix(1, 1),
7527 this);
7528 fail("IllegalArgumentException expected but not thrown");
7529 } catch (final IllegalArgumentException ignore) {
7530 }
7531 try {
7532 calibrator = new KnownGravityNormAccelerometerCalibrator(
7533 true, new Matrix(1, 3),
7534 this);
7535 fail("IllegalArgumentException expected but not thrown");
7536 } catch (final IllegalArgumentException ignore) {
7537 }
7538 assertNull(calibrator);
7539 }
7540
7541 @Test
7542 public void testConstructor69() throws WrongSizeException {
7543 final Collection<StandardDeviationBodyKinematics> measurements =
7544 Collections.emptyList();
7545
7546 final Matrix ba = generateBa();
7547 final double biasX = ba.getElementAtIndex(0);
7548 final double biasY = ba.getElementAtIndex(1);
7549 final double biasZ = ba.getElementAtIndex(2);
7550
7551 KnownGravityNormAccelerometerCalibrator calibrator =
7552 new KnownGravityNormAccelerometerCalibrator(measurements,
7553 true, ba);
7554
7555
7556 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7557 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7558 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7559 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7560 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7561 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7562 final Acceleration bx2 = new Acceleration(0.0,
7563 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7564 calibrator.getInitialBiasXAsAcceleration(bx2);
7565 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7566 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7567 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7568 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7569 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7570 final Acceleration by2 = new Acceleration(0.0,
7571 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7572 calibrator.getInitialBiasYAsAcceleration(by2);
7573 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7574 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7575 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7576 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7577 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7578 final Acceleration bz2 = new Acceleration(0.0,
7579 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7580 calibrator.getInitialBiasZAsAcceleration(bz2);
7581 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7582 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7583 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7584 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7585 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7586 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7587 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7588 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7589 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7590 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7591 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7592 final double[] bias1 = calibrator.getInitialBias();
7593 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7594 final double[] bias2 = new double[3];
7595 calibrator.getInitialBias(bias2);
7596 assertArrayEquals(bias1, bias2, 0.0);
7597 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7598 assertEquals(b1, ba);
7599 final Matrix b2 = new Matrix(3, 1);
7600 calibrator.getInitialBiasAsMatrix(b2);
7601 assertEquals(b1, b2);
7602 final Matrix ma1 = calibrator.getInitialMa();
7603 assertEquals(ma1, new Matrix(3, 3));
7604 final Matrix ma2 = new Matrix(3, 3);
7605 calibrator.getInitialMa(ma2);
7606 assertEquals(ma1, ma2);
7607 assertSame(calibrator.getMeasurements(), measurements);
7608 assertTrue(calibrator.isCommonAxisUsed());
7609 assertNull(calibrator.getListener());
7610 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7611 assertFalse(calibrator.isReady());
7612 assertFalse(calibrator.isRunning());
7613 assertNull(calibrator.getEstimatedBiases());
7614 assertFalse(calibrator.getEstimatedBiases(null));
7615 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7616 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7617 assertNull(calibrator.getEstimatedBiasFx());
7618 assertNull(calibrator.getEstimatedBiasFy());
7619 assertNull(calibrator.getEstimatedBiasFz());
7620 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7621 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7622 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7623 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7624 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7625 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7626 assertNull(calibrator.getEstimatedMa());
7627 assertNull(calibrator.getEstimatedSx());
7628 assertNull(calibrator.getEstimatedSy());
7629 assertNull(calibrator.getEstimatedSz());
7630 assertNull(calibrator.getEstimatedMxy());
7631 assertNull(calibrator.getEstimatedMxz());
7632 assertNull(calibrator.getEstimatedMyx());
7633 assertNull(calibrator.getEstimatedMyz());
7634 assertNull(calibrator.getEstimatedMzx());
7635 assertNull(calibrator.getEstimatedMzy());
7636 assertNull(calibrator.getEstimatedCovariance());
7637 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7638 assertNull(calibrator.getGroundTruthGravityNorm());
7639 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7640 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7641
7642
7643 calibrator = null;
7644 try {
7645 calibrator = new KnownGravityNormAccelerometerCalibrator(
7646 measurements, true,
7647 new Matrix(1, 1));
7648 fail("IllegalArgumentException expected but not thrown");
7649 } catch (final IllegalArgumentException ignore) {
7650 }
7651 try {
7652 calibrator = new KnownGravityNormAccelerometerCalibrator(
7653 measurements, true,
7654 new Matrix(1, 3));
7655 fail("IllegalArgumentException expected but not thrown");
7656 } catch (final IllegalArgumentException ignore) {
7657 }
7658 assertNull(calibrator);
7659 }
7660
7661 @Test
7662 public void testConstructor70() throws WrongSizeException {
7663 final Collection<StandardDeviationBodyKinematics> measurements =
7664 Collections.emptyList();
7665
7666 final Matrix ba = generateBa();
7667 final double biasX = ba.getElementAtIndex(0);
7668 final double biasY = ba.getElementAtIndex(1);
7669 final double biasZ = ba.getElementAtIndex(2);
7670
7671 KnownGravityNormAccelerometerCalibrator calibrator =
7672 new KnownGravityNormAccelerometerCalibrator(measurements,
7673 true, ba, this);
7674
7675
7676 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7677 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7678 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7679 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7680 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7681 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7682 final Acceleration bx2 = new Acceleration(0.0,
7683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7684 calibrator.getInitialBiasXAsAcceleration(bx2);
7685 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7686 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7687 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7688 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7689 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7690 final Acceleration by2 = new Acceleration(0.0,
7691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7692 calibrator.getInitialBiasYAsAcceleration(by2);
7693 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7694 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7695 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7696 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7697 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7698 final Acceleration bz2 = new Acceleration(0.0,
7699 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7700 calibrator.getInitialBiasZAsAcceleration(bz2);
7701 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7702 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7703 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7704 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7705 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7706 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7707 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7708 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7709 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7710 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7711 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7712 final double[] bias1 = calibrator.getInitialBias();
7713 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7714 final double[] bias2 = new double[3];
7715 calibrator.getInitialBias(bias2);
7716 assertArrayEquals(bias1, bias2, 0.0);
7717 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7718 assertEquals(b1, ba);
7719 final Matrix b2 = new Matrix(3, 1);
7720 calibrator.getInitialBiasAsMatrix(b2);
7721 assertEquals(b1, b2);
7722 final Matrix ma1 = calibrator.getInitialMa();
7723 assertEquals(ma1, new Matrix(3, 3));
7724 final Matrix ma2 = new Matrix(3, 3);
7725 calibrator.getInitialMa(ma2);
7726 assertEquals(ma1, ma2);
7727 assertSame(calibrator.getMeasurements(), measurements);
7728 assertTrue(calibrator.isCommonAxisUsed());
7729 assertSame(calibrator.getListener(), this);
7730 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7731 assertFalse(calibrator.isReady());
7732 assertFalse(calibrator.isRunning());
7733 assertNull(calibrator.getEstimatedBiases());
7734 assertFalse(calibrator.getEstimatedBiases(null));
7735 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7736 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7737 assertNull(calibrator.getEstimatedBiasFx());
7738 assertNull(calibrator.getEstimatedBiasFy());
7739 assertNull(calibrator.getEstimatedBiasFz());
7740 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7741 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7742 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7743 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7744 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7745 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7746 assertNull(calibrator.getEstimatedMa());
7747 assertNull(calibrator.getEstimatedSx());
7748 assertNull(calibrator.getEstimatedSy());
7749 assertNull(calibrator.getEstimatedSz());
7750 assertNull(calibrator.getEstimatedMxy());
7751 assertNull(calibrator.getEstimatedMxz());
7752 assertNull(calibrator.getEstimatedMyx());
7753 assertNull(calibrator.getEstimatedMyz());
7754 assertNull(calibrator.getEstimatedMzx());
7755 assertNull(calibrator.getEstimatedMzy());
7756 assertNull(calibrator.getEstimatedCovariance());
7757 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7758 assertNull(calibrator.getGroundTruthGravityNorm());
7759 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7760 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7761
7762
7763 calibrator = null;
7764 try {
7765 calibrator = new KnownGravityNormAccelerometerCalibrator(
7766 measurements, true,
7767 new Matrix(1, 1), this);
7768 fail("IllegalArgumentException expected but not thrown");
7769 } catch (final IllegalArgumentException ignore) {
7770 }
7771 try {
7772 calibrator = new KnownGravityNormAccelerometerCalibrator(
7773 measurements, true,
7774 new Matrix(1, 3), this);
7775 fail("IllegalArgumentException expected but not thrown");
7776 } catch (final IllegalArgumentException ignore) {
7777 }
7778 assertNull(calibrator);
7779 }
7780
7781 @Test
7782 public void testConstructor71() throws WrongSizeException {
7783 final Matrix ba = generateBa();
7784 final double biasX = ba.getElementAtIndex(0);
7785 final double biasY = ba.getElementAtIndex(1);
7786 final double biasZ = ba.getElementAtIndex(2);
7787
7788 final Matrix ma = generateMaCommonAxis();
7789 final double sx = ma.getElementAt(0, 0);
7790 final double sy = ma.getElementAt(1, 1);
7791 final double sz = ma.getElementAt(2, 2);
7792 final double mxy = ma.getElementAt(0, 1);
7793 final double mxz = ma.getElementAt(0, 2);
7794 final double myx = ma.getElementAt(1, 0);
7795 final double myz = ma.getElementAt(1, 2);
7796 final double mzx = ma.getElementAt(2, 0);
7797 final double mzy = ma.getElementAt(2, 1);
7798
7799 KnownGravityNormAccelerometerCalibrator calibrator =
7800 new KnownGravityNormAccelerometerCalibrator(ba, ma);
7801
7802
7803 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7804 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7805 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7806 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7807 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7808 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7809 final Acceleration bx2 = new Acceleration(0.0,
7810 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7811 calibrator.getInitialBiasXAsAcceleration(bx2);
7812 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7813 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7814 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7815 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7816 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7817 final Acceleration by2 = new Acceleration(0.0,
7818 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7819 calibrator.getInitialBiasYAsAcceleration(by2);
7820 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7821 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7822 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7823 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7824 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7825 final Acceleration bz2 = new Acceleration(0.0,
7826 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7827 calibrator.getInitialBiasZAsAcceleration(bz2);
7828 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7829 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7830 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7831 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7832 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7833 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7834 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7835 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7836 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7837 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7838 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7839 final double[] bias1 = calibrator.getInitialBias();
7840 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7841 final double[] bias2 = new double[3];
7842 calibrator.getInitialBias(bias2);
7843 assertArrayEquals(bias1, bias2, 0.0);
7844 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7845 assertEquals(b1, ba);
7846 final Matrix b2 = new Matrix(3, 1);
7847 calibrator.getInitialBiasAsMatrix(b2);
7848 assertEquals(b1, b2);
7849 final Matrix ma1 = new Matrix(3, 3);
7850 ma1.setSubmatrix(0, 0,
7851 2, 2,
7852 new double[]{sx, myx, mzx,
7853 mxy, sy, mzy,
7854 mxz, myz, sz});
7855 assertEquals(calibrator.getInitialMa(), ma1);
7856 final Matrix ma2 = new Matrix(3, 3);
7857 calibrator.getInitialMa(ma2);
7858 assertEquals(ma1, ma2);
7859 assertNull(calibrator.getMeasurements());
7860 assertFalse(calibrator.isCommonAxisUsed());
7861 assertNull(calibrator.getListener());
7862 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7863 assertFalse(calibrator.isReady());
7864 assertFalse(calibrator.isRunning());
7865 assertNull(calibrator.getEstimatedBiases());
7866 assertFalse(calibrator.getEstimatedBiases(null));
7867 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7868 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7869 assertNull(calibrator.getEstimatedBiasFx());
7870 assertNull(calibrator.getEstimatedBiasFy());
7871 assertNull(calibrator.getEstimatedBiasFz());
7872 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7873 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7874 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7875 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7876 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7877 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7878 assertNull(calibrator.getEstimatedMa());
7879 assertNull(calibrator.getEstimatedSx());
7880 assertNull(calibrator.getEstimatedSy());
7881 assertNull(calibrator.getEstimatedSz());
7882 assertNull(calibrator.getEstimatedMxy());
7883 assertNull(calibrator.getEstimatedMxz());
7884 assertNull(calibrator.getEstimatedMyx());
7885 assertNull(calibrator.getEstimatedMyz());
7886 assertNull(calibrator.getEstimatedMzx());
7887 assertNull(calibrator.getEstimatedMzy());
7888 assertNull(calibrator.getEstimatedCovariance());
7889 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7890 assertNull(calibrator.getGroundTruthGravityNorm());
7891 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7892 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7893
7894
7895 calibrator = null;
7896 try {
7897 calibrator = new KnownGravityNormAccelerometerCalibrator(
7898 new Matrix(1, 1), ma);
7899 fail("IllegalArgumentException expected but not thrown");
7900 } catch (final IllegalArgumentException ignore) {
7901 }
7902 try {
7903 calibrator = new KnownGravityNormAccelerometerCalibrator(
7904 new Matrix(1, 3), ma);
7905 fail("IllegalArgumentException expected but not thrown");
7906 } catch (final IllegalArgumentException ignore) {
7907 }
7908 try {
7909 calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
7910 new Matrix(1, 3));
7911 fail("IllegalArgumentException expected but not thrown");
7912 } catch (final IllegalArgumentException ignore) {
7913 }
7914 try {
7915 calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
7916 new Matrix(3, 1));
7917 fail("IllegalArgumentException expected but not thrown");
7918 } catch (final IllegalArgumentException ignore) {
7919 }
7920 assertNull(calibrator);
7921 }
7922
7923 @Test
7924 public void testConstructor72() throws WrongSizeException {
7925 final Matrix ba = generateBa();
7926 final double biasX = ba.getElementAtIndex(0);
7927 final double biasY = ba.getElementAtIndex(1);
7928 final double biasZ = ba.getElementAtIndex(2);
7929
7930 final Matrix ma = generateMaCommonAxis();
7931 final double sx = ma.getElementAt(0, 0);
7932 final double sy = ma.getElementAt(1, 1);
7933 final double sz = ma.getElementAt(2, 2);
7934 final double mxy = ma.getElementAt(0, 1);
7935 final double mxz = ma.getElementAt(0, 2);
7936 final double myx = ma.getElementAt(1, 0);
7937 final double myz = ma.getElementAt(1, 2);
7938 final double mzx = ma.getElementAt(2, 0);
7939 final double mzy = ma.getElementAt(2, 1);
7940
7941 KnownGravityNormAccelerometerCalibrator calibrator =
7942 new KnownGravityNormAccelerometerCalibrator(
7943 ba, ma, this);
7944
7945
7946 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7947 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7948 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7949 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7950 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7951 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7952 final Acceleration bx2 = new Acceleration(0.0,
7953 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7954 calibrator.getInitialBiasXAsAcceleration(bx2);
7955 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7956 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7957 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7958 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7959 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7960 final Acceleration by2 = new Acceleration(0.0,
7961 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7962 calibrator.getInitialBiasYAsAcceleration(by2);
7963 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7964 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7965 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7966 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7967 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7968 final Acceleration bz2 = new Acceleration(0.0,
7969 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7970 calibrator.getInitialBiasZAsAcceleration(bz2);
7971 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7972 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7973 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7974 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7975 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7976 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7977 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7978 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7979 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7980 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7981 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7982 final double[] bias1 = calibrator.getInitialBias();
7983 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7984 final double[] bias2 = new double[3];
7985 calibrator.getInitialBias(bias2);
7986 assertArrayEquals(bias1, bias2, 0.0);
7987 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7988 assertEquals(b1, ba);
7989 final Matrix b2 = new Matrix(3, 1);
7990 calibrator.getInitialBiasAsMatrix(b2);
7991 assertEquals(b1, b2);
7992 final Matrix ma1 = new Matrix(3, 3);
7993 ma1.setSubmatrix(0, 0,
7994 2, 2,
7995 new double[]{sx, myx, mzx,
7996 mxy, sy, mzy,
7997 mxz, myz, sz});
7998 assertEquals(calibrator.getInitialMa(), ma1);
7999 final Matrix ma2 = new Matrix(3, 3);
8000 calibrator.getInitialMa(ma2);
8001 assertEquals(ma1, ma2);
8002 assertNull(calibrator.getMeasurements());
8003 assertFalse(calibrator.isCommonAxisUsed());
8004 assertSame(calibrator.getListener(), this);
8005 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8006 assertFalse(calibrator.isReady());
8007 assertFalse(calibrator.isRunning());
8008 assertNull(calibrator.getEstimatedBiases());
8009 assertFalse(calibrator.getEstimatedBiases(null));
8010 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8011 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8012 assertNull(calibrator.getEstimatedBiasFx());
8013 assertNull(calibrator.getEstimatedBiasFy());
8014 assertNull(calibrator.getEstimatedBiasFz());
8015 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8016 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8017 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8018 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8019 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8020 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8021 assertNull(calibrator.getEstimatedMa());
8022 assertNull(calibrator.getEstimatedSx());
8023 assertNull(calibrator.getEstimatedSy());
8024 assertNull(calibrator.getEstimatedSz());
8025 assertNull(calibrator.getEstimatedMxy());
8026 assertNull(calibrator.getEstimatedMxz());
8027 assertNull(calibrator.getEstimatedMyx());
8028 assertNull(calibrator.getEstimatedMyz());
8029 assertNull(calibrator.getEstimatedMzx());
8030 assertNull(calibrator.getEstimatedMzy());
8031 assertNull(calibrator.getEstimatedCovariance());
8032 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8033 assertNull(calibrator.getGroundTruthGravityNorm());
8034 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8035 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8036
8037
8038 calibrator = null;
8039 try {
8040 calibrator = new KnownGravityNormAccelerometerCalibrator(
8041 new Matrix(1, 1), ma, this);
8042 fail("IllegalArgumentException expected but not thrown");
8043 } catch (final IllegalArgumentException ignore) {
8044 }
8045 try {
8046 calibrator = new KnownGravityNormAccelerometerCalibrator(
8047 new Matrix(1, 3), ma, this);
8048 fail("IllegalArgumentException expected but not thrown");
8049 } catch (final IllegalArgumentException ignore) {
8050 }
8051 try {
8052 calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
8053 new Matrix(1, 3), this);
8054 fail("IllegalArgumentException expected but not thrown");
8055 } catch (final IllegalArgumentException ignore) {
8056 }
8057 try {
8058 calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
8059 new Matrix(3, 1), this);
8060 fail("IllegalArgumentException expected but not thrown");
8061 } catch (final IllegalArgumentException ignore) {
8062 }
8063 assertNull(calibrator);
8064 }
8065
8066 @Test
8067 public void testConstructor73() throws WrongSizeException {
8068 final Collection<StandardDeviationBodyKinematics> measurements =
8069 Collections.emptyList();
8070
8071 final Matrix ba = generateBa();
8072 final double biasX = ba.getElementAtIndex(0);
8073 final double biasY = ba.getElementAtIndex(1);
8074 final double biasZ = ba.getElementAtIndex(2);
8075
8076 final Matrix ma = generateMaCommonAxis();
8077 final double sx = ma.getElementAt(0, 0);
8078 final double sy = ma.getElementAt(1, 1);
8079 final double sz = ma.getElementAt(2, 2);
8080 final double mxy = ma.getElementAt(0, 1);
8081 final double mxz = ma.getElementAt(0, 2);
8082 final double myx = ma.getElementAt(1, 0);
8083 final double myz = ma.getElementAt(1, 2);
8084 final double mzx = ma.getElementAt(2, 0);
8085 final double mzy = ma.getElementAt(2, 1);
8086
8087 KnownGravityNormAccelerometerCalibrator calibrator =
8088 new KnownGravityNormAccelerometerCalibrator(
8089 measurements, ba, ma);
8090
8091
8092 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8093 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8094 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8095 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8096 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8097 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8098 final Acceleration bx2 = new Acceleration(0.0,
8099 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8100 calibrator.getInitialBiasXAsAcceleration(bx2);
8101 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8102 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8103 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8104 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8105 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8106 final Acceleration by2 = new Acceleration(0.0,
8107 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8108 calibrator.getInitialBiasYAsAcceleration(by2);
8109 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8110 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8111 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8112 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8113 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8114 final Acceleration bz2 = new Acceleration(0.0,
8115 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8116 calibrator.getInitialBiasZAsAcceleration(bz2);
8117 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8118 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8119 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8120 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8121 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8122 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8123 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8124 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8125 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8126 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8127 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8128 final double[] bias1 = calibrator.getInitialBias();
8129 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8130 final double[] bias2 = new double[3];
8131 calibrator.getInitialBias(bias2);
8132 assertArrayEquals(bias1, bias2, 0.0);
8133 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8134 assertEquals(b1, ba);
8135 final Matrix b2 = new Matrix(3, 1);
8136 calibrator.getInitialBiasAsMatrix(b2);
8137 assertEquals(b1, b2);
8138 final Matrix ma1 = new Matrix(3, 3);
8139 ma1.setSubmatrix(0, 0,
8140 2, 2,
8141 new double[]{sx, myx, mzx,
8142 mxy, sy, mzy,
8143 mxz, myz, sz});
8144 assertEquals(calibrator.getInitialMa(), ma1);
8145 final Matrix ma2 = new Matrix(3, 3);
8146 calibrator.getInitialMa(ma2);
8147 assertEquals(ma1, ma2);
8148 assertSame(calibrator.getMeasurements(), measurements);
8149 assertFalse(calibrator.isCommonAxisUsed());
8150 assertNull(calibrator.getListener());
8151 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8152 assertFalse(calibrator.isReady());
8153 assertFalse(calibrator.isRunning());
8154 assertNull(calibrator.getEstimatedBiases());
8155 assertFalse(calibrator.getEstimatedBiases(null));
8156 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8157 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8158 assertNull(calibrator.getEstimatedBiasFx());
8159 assertNull(calibrator.getEstimatedBiasFy());
8160 assertNull(calibrator.getEstimatedBiasFz());
8161 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8162 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8163 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8164 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8165 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8166 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8167 assertNull(calibrator.getEstimatedMa());
8168 assertNull(calibrator.getEstimatedSx());
8169 assertNull(calibrator.getEstimatedSy());
8170 assertNull(calibrator.getEstimatedSz());
8171 assertNull(calibrator.getEstimatedMxy());
8172 assertNull(calibrator.getEstimatedMxz());
8173 assertNull(calibrator.getEstimatedMyx());
8174 assertNull(calibrator.getEstimatedMyz());
8175 assertNull(calibrator.getEstimatedMzx());
8176 assertNull(calibrator.getEstimatedMzy());
8177 assertNull(calibrator.getEstimatedCovariance());
8178 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8179 assertNull(calibrator.getGroundTruthGravityNorm());
8180 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8181 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8182
8183
8184 calibrator = null;
8185 try {
8186 calibrator = new KnownGravityNormAccelerometerCalibrator(
8187 measurements, new Matrix(1, 1), ma);
8188 fail("IllegalArgumentException expected but not thrown");
8189 } catch (final IllegalArgumentException ignore) {
8190 }
8191 try {
8192 calibrator = new KnownGravityNormAccelerometerCalibrator(
8193 measurements, new Matrix(1, 3), ma);
8194 fail("IllegalArgumentException expected but not thrown");
8195 } catch (final IllegalArgumentException ignore) {
8196 }
8197 try {
8198 calibrator = new KnownGravityNormAccelerometerCalibrator(
8199 measurements, ba, new Matrix(1, 3));
8200 fail("IllegalArgumentException expected but not thrown");
8201 } catch (final IllegalArgumentException ignore) {
8202 }
8203 try {
8204 calibrator = new KnownGravityNormAccelerometerCalibrator(
8205 measurements, ba, new Matrix(3, 1));
8206 fail("IllegalArgumentException expected but not thrown");
8207 } catch (final IllegalArgumentException ignore) {
8208 }
8209 assertNull(calibrator);
8210 }
8211
8212 @Test
8213 public void testConstructor74() throws WrongSizeException {
8214 final Collection<StandardDeviationBodyKinematics> measurements =
8215 Collections.emptyList();
8216
8217 final Matrix ba = generateBa();
8218 final double biasX = ba.getElementAtIndex(0);
8219 final double biasY = ba.getElementAtIndex(1);
8220 final double biasZ = ba.getElementAtIndex(2);
8221
8222 final Matrix ma = generateMaCommonAxis();
8223 final double sx = ma.getElementAt(0, 0);
8224 final double sy = ma.getElementAt(1, 1);
8225 final double sz = ma.getElementAt(2, 2);
8226 final double mxy = ma.getElementAt(0, 1);
8227 final double mxz = ma.getElementAt(0, 2);
8228 final double myx = ma.getElementAt(1, 0);
8229 final double myz = ma.getElementAt(1, 2);
8230 final double mzx = ma.getElementAt(2, 0);
8231 final double mzy = ma.getElementAt(2, 1);
8232
8233 KnownGravityNormAccelerometerCalibrator calibrator =
8234 new KnownGravityNormAccelerometerCalibrator(
8235 measurements, ba, ma, this);
8236
8237
8238 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8239 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8240 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8241 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8242 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8243 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8244 final Acceleration bx2 = new Acceleration(0.0,
8245 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8246 calibrator.getInitialBiasXAsAcceleration(bx2);
8247 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8248 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8249 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8250 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8251 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8252 final Acceleration by2 = new Acceleration(0.0,
8253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8254 calibrator.getInitialBiasYAsAcceleration(by2);
8255 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8256 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8257 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8258 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8259 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8260 final Acceleration bz2 = new Acceleration(0.0,
8261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8262 calibrator.getInitialBiasZAsAcceleration(bz2);
8263 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8264 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8265 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8266 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8267 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8268 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8269 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8270 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8271 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8272 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8273 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8274 final double[] bias1 = calibrator.getInitialBias();
8275 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8276 final double[] bias2 = new double[3];
8277 calibrator.getInitialBias(bias2);
8278 assertArrayEquals(bias1, bias2, 0.0);
8279 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8280 assertEquals(b1, ba);
8281 final Matrix b2 = new Matrix(3, 1);
8282 calibrator.getInitialBiasAsMatrix(b2);
8283 assertEquals(b1, b2);
8284 final Matrix ma1 = new Matrix(3, 3);
8285 ma1.setSubmatrix(0, 0,
8286 2, 2,
8287 new double[]{sx, myx, mzx,
8288 mxy, sy, mzy,
8289 mxz, myz, sz});
8290 assertEquals(calibrator.getInitialMa(), ma1);
8291 final Matrix ma2 = new Matrix(3, 3);
8292 calibrator.getInitialMa(ma2);
8293 assertEquals(ma1, ma2);
8294 assertSame(calibrator.getMeasurements(), measurements);
8295 assertFalse(calibrator.isCommonAxisUsed());
8296 assertSame(calibrator.getListener(), this);
8297 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8298 assertFalse(calibrator.isReady());
8299 assertFalse(calibrator.isRunning());
8300 assertNull(calibrator.getEstimatedBiases());
8301 assertFalse(calibrator.getEstimatedBiases(null));
8302 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8303 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8304 assertNull(calibrator.getEstimatedBiasFx());
8305 assertNull(calibrator.getEstimatedBiasFy());
8306 assertNull(calibrator.getEstimatedBiasFz());
8307 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8308 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8309 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8310 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8311 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8312 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8313 assertNull(calibrator.getEstimatedMa());
8314 assertNull(calibrator.getEstimatedSx());
8315 assertNull(calibrator.getEstimatedSy());
8316 assertNull(calibrator.getEstimatedSz());
8317 assertNull(calibrator.getEstimatedMxy());
8318 assertNull(calibrator.getEstimatedMxz());
8319 assertNull(calibrator.getEstimatedMyx());
8320 assertNull(calibrator.getEstimatedMyz());
8321 assertNull(calibrator.getEstimatedMzx());
8322 assertNull(calibrator.getEstimatedMzy());
8323 assertNull(calibrator.getEstimatedCovariance());
8324 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8325 assertNull(calibrator.getGroundTruthGravityNorm());
8326 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8327 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8328
8329
8330 calibrator = null;
8331 try {
8332 calibrator = new KnownGravityNormAccelerometerCalibrator(
8333 measurements, new Matrix(1, 1), ma,
8334 this);
8335 fail("IllegalArgumentException expected but not thrown");
8336 } catch (final IllegalArgumentException ignore) {
8337 }
8338 try {
8339 calibrator = new KnownGravityNormAccelerometerCalibrator(
8340 measurements, new Matrix(1, 3), ma,
8341 this);
8342 fail("IllegalArgumentException expected but not thrown");
8343 } catch (final IllegalArgumentException ignore) {
8344 }
8345 try {
8346 calibrator = new KnownGravityNormAccelerometerCalibrator(
8347 measurements, ba, new Matrix(1, 3),
8348 this);
8349 fail("IllegalArgumentException expected but not thrown");
8350 } catch (final IllegalArgumentException ignore) {
8351 }
8352 try {
8353 calibrator = new KnownGravityNormAccelerometerCalibrator(
8354 measurements, ba, new Matrix(3, 1),
8355 this);
8356 fail("IllegalArgumentException expected but not thrown");
8357 } catch (final IllegalArgumentException ignore) {
8358 }
8359 assertNull(calibrator);
8360 }
8361
8362 @Test
8363 public void testConstructor75() throws WrongSizeException {
8364 final Matrix ba = generateBa();
8365 final double biasX = ba.getElementAtIndex(0);
8366 final double biasY = ba.getElementAtIndex(1);
8367 final double biasZ = ba.getElementAtIndex(2);
8368
8369 final Matrix ma = generateMaCommonAxis();
8370 final double sx = ma.getElementAt(0, 0);
8371 final double sy = ma.getElementAt(1, 1);
8372 final double sz = ma.getElementAt(2, 2);
8373 final double mxy = ma.getElementAt(0, 1);
8374 final double mxz = ma.getElementAt(0, 2);
8375 final double myx = ma.getElementAt(1, 0);
8376 final double myz = ma.getElementAt(1, 2);
8377 final double mzx = ma.getElementAt(2, 0);
8378 final double mzy = ma.getElementAt(2, 1);
8379
8380 KnownGravityNormAccelerometerCalibrator calibrator =
8381 new KnownGravityNormAccelerometerCalibrator(true, ba, ma);
8382
8383
8384 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8385 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8386 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8387 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8388 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8389 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8390 final Acceleration bx2 = new Acceleration(0.0,
8391 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8392 calibrator.getInitialBiasXAsAcceleration(bx2);
8393 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8394 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8395 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8396 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8397 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8398 final Acceleration by2 = new Acceleration(0.0,
8399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8400 calibrator.getInitialBiasYAsAcceleration(by2);
8401 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8402 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8403 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8404 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8405 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8406 final Acceleration bz2 = new Acceleration(0.0,
8407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8408 calibrator.getInitialBiasZAsAcceleration(bz2);
8409 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8410 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8411 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8412 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8413 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8414 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8415 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8416 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8417 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8418 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8419 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8420 final double[] bias1 = calibrator.getInitialBias();
8421 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8422 final double[] bias2 = new double[3];
8423 calibrator.getInitialBias(bias2);
8424 assertArrayEquals(bias1, bias2, 0.0);
8425 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8426 assertEquals(b1, ba);
8427 final Matrix b2 = new Matrix(3, 1);
8428 calibrator.getInitialBiasAsMatrix(b2);
8429 assertEquals(b1, b2);
8430 final Matrix ma1 = new Matrix(3, 3);
8431 ma1.setSubmatrix(0, 0,
8432 2, 2,
8433 new double[]{sx, myx, mzx,
8434 mxy, sy, mzy,
8435 mxz, myz, sz});
8436 assertEquals(calibrator.getInitialMa(), ma1);
8437 final Matrix ma2 = new Matrix(3, 3);
8438 calibrator.getInitialMa(ma2);
8439 assertEquals(ma1, ma2);
8440 assertNull(calibrator.getMeasurements());
8441 assertTrue(calibrator.isCommonAxisUsed());
8442 assertNull(calibrator.getListener());
8443 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8444 assertFalse(calibrator.isReady());
8445 assertFalse(calibrator.isRunning());
8446 assertNull(calibrator.getEstimatedBiases());
8447 assertFalse(calibrator.getEstimatedBiases(null));
8448 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8449 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8450 assertNull(calibrator.getEstimatedBiasFx());
8451 assertNull(calibrator.getEstimatedBiasFy());
8452 assertNull(calibrator.getEstimatedBiasFz());
8453 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8454 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8455 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8456 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8457 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8458 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8459 assertNull(calibrator.getEstimatedMa());
8460 assertNull(calibrator.getEstimatedSx());
8461 assertNull(calibrator.getEstimatedSy());
8462 assertNull(calibrator.getEstimatedSz());
8463 assertNull(calibrator.getEstimatedMxy());
8464 assertNull(calibrator.getEstimatedMxz());
8465 assertNull(calibrator.getEstimatedMyx());
8466 assertNull(calibrator.getEstimatedMyz());
8467 assertNull(calibrator.getEstimatedMzx());
8468 assertNull(calibrator.getEstimatedMzy());
8469 assertNull(calibrator.getEstimatedCovariance());
8470 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8471 assertNull(calibrator.getGroundTruthGravityNorm());
8472 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8473 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8474
8475
8476 calibrator = null;
8477 try {
8478 calibrator = new KnownGravityNormAccelerometerCalibrator(
8479 true, new Matrix(1, 1), ma);
8480 fail("IllegalArgumentException expected but not thrown");
8481 } catch (final IllegalArgumentException ignore) {
8482 }
8483 try {
8484 calibrator = new KnownGravityNormAccelerometerCalibrator(
8485 true, new Matrix(1, 3), ma);
8486 fail("IllegalArgumentException expected but not thrown");
8487 } catch (final IllegalArgumentException ignore) {
8488 }
8489 try {
8490 calibrator = new KnownGravityNormAccelerometerCalibrator(
8491 true, ba, new Matrix(1, 3));
8492 fail("IllegalArgumentException expected but not thrown");
8493 } catch (final IllegalArgumentException ignore) {
8494 }
8495 try {
8496 calibrator = new KnownGravityNormAccelerometerCalibrator(
8497 true, ba, new Matrix(3, 1));
8498 fail("IllegalArgumentException expected but not thrown");
8499 } catch (final IllegalArgumentException ignore) {
8500 }
8501 assertNull(calibrator);
8502 }
8503
8504 @Test
8505 public void testConstructor76() throws WrongSizeException {
8506 final Matrix ba = generateBa();
8507 final double biasX = ba.getElementAtIndex(0);
8508 final double biasY = ba.getElementAtIndex(1);
8509 final double biasZ = ba.getElementAtIndex(2);
8510
8511 final Matrix ma = generateMaCommonAxis();
8512 final double sx = ma.getElementAt(0, 0);
8513 final double sy = ma.getElementAt(1, 1);
8514 final double sz = ma.getElementAt(2, 2);
8515 final double mxy = ma.getElementAt(0, 1);
8516 final double mxz = ma.getElementAt(0, 2);
8517 final double myx = ma.getElementAt(1, 0);
8518 final double myz = ma.getElementAt(1, 2);
8519 final double mzx = ma.getElementAt(2, 0);
8520 final double mzy = ma.getElementAt(2, 1);
8521
8522 KnownGravityNormAccelerometerCalibrator calibrator =
8523 new KnownGravityNormAccelerometerCalibrator(
8524 true, ba, ma, this);
8525
8526
8527 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8528 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8529 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8530 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8531 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8532 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8533 final Acceleration bx2 = new Acceleration(0.0,
8534 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8535 calibrator.getInitialBiasXAsAcceleration(bx2);
8536 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8537 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8538 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8539 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8540 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8541 final Acceleration by2 = new Acceleration(0.0,
8542 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8543 calibrator.getInitialBiasYAsAcceleration(by2);
8544 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8545 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8546 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8547 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8548 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8549 final Acceleration bz2 = new Acceleration(0.0,
8550 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8551 calibrator.getInitialBiasZAsAcceleration(bz2);
8552 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8553 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8554 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8555 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8556 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8557 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8558 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8559 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8560 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8561 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8562 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8563 final double[] bias1 = calibrator.getInitialBias();
8564 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8565 final double[] bias2 = new double[3];
8566 calibrator.getInitialBias(bias2);
8567 assertArrayEquals(bias1, bias2, 0.0);
8568 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8569 assertEquals(b1, ba);
8570 final Matrix b2 = new Matrix(3, 1);
8571 calibrator.getInitialBiasAsMatrix(b2);
8572 assertEquals(b1, b2);
8573 final Matrix ma1 = new Matrix(3, 3);
8574 ma1.setSubmatrix(0, 0,
8575 2, 2,
8576 new double[]{sx, myx, mzx,
8577 mxy, sy, mzy,
8578 mxz, myz, sz});
8579 assertEquals(calibrator.getInitialMa(), ma1);
8580 final Matrix ma2 = new Matrix(3, 3);
8581 calibrator.getInitialMa(ma2);
8582 assertEquals(ma1, ma2);
8583 assertNull(calibrator.getMeasurements());
8584 assertTrue(calibrator.isCommonAxisUsed());
8585 assertSame(calibrator.getListener(), this);
8586 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8587 assertFalse(calibrator.isReady());
8588 assertFalse(calibrator.isRunning());
8589 assertNull(calibrator.getEstimatedBiases());
8590 assertFalse(calibrator.getEstimatedBiases(null));
8591 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8592 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8593 assertNull(calibrator.getEstimatedBiasFx());
8594 assertNull(calibrator.getEstimatedBiasFy());
8595 assertNull(calibrator.getEstimatedBiasFz());
8596 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8597 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8598 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8599 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8600 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8601 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8602 assertNull(calibrator.getEstimatedMa());
8603 assertNull(calibrator.getEstimatedSx());
8604 assertNull(calibrator.getEstimatedSy());
8605 assertNull(calibrator.getEstimatedSz());
8606 assertNull(calibrator.getEstimatedMxy());
8607 assertNull(calibrator.getEstimatedMxz());
8608 assertNull(calibrator.getEstimatedMyx());
8609 assertNull(calibrator.getEstimatedMyz());
8610 assertNull(calibrator.getEstimatedMzx());
8611 assertNull(calibrator.getEstimatedMzy());
8612 assertNull(calibrator.getEstimatedCovariance());
8613 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8614 assertNull(calibrator.getGroundTruthGravityNorm());
8615 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8616 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8617
8618
8619 calibrator = null;
8620 try {
8621 calibrator = new KnownGravityNormAccelerometerCalibrator(
8622 true, new Matrix(1, 1), ma,
8623 this);
8624 fail("IllegalArgumentException expected but not thrown");
8625 } catch (final IllegalArgumentException ignore) {
8626 }
8627 try {
8628 calibrator = new KnownGravityNormAccelerometerCalibrator(
8629 true, new Matrix(1, 3), ma,
8630 this);
8631 fail("IllegalArgumentException expected but not thrown");
8632 } catch (final IllegalArgumentException ignore) {
8633 }
8634 try {
8635 calibrator = new KnownGravityNormAccelerometerCalibrator(
8636 true, ba, new Matrix(1, 3),
8637 this);
8638 fail("IllegalArgumentException expected but not thrown");
8639 } catch (final IllegalArgumentException ignore) {
8640 }
8641 try {
8642 calibrator = new KnownGravityNormAccelerometerCalibrator(
8643 true, ba, new Matrix(3, 1),
8644 this);
8645 fail("IllegalArgumentException expected but not thrown");
8646 } catch (final IllegalArgumentException ignore) {
8647 }
8648 assertNull(calibrator);
8649 }
8650
8651 @Test
8652 public void testConstructor77() throws WrongSizeException {
8653 final Collection<StandardDeviationBodyKinematics> measurements =
8654 Collections.emptyList();
8655
8656 final Matrix ba = generateBa();
8657 final double biasX = ba.getElementAtIndex(0);
8658 final double biasY = ba.getElementAtIndex(1);
8659 final double biasZ = ba.getElementAtIndex(2);
8660
8661 final Matrix ma = generateMaCommonAxis();
8662 final double sx = ma.getElementAt(0, 0);
8663 final double sy = ma.getElementAt(1, 1);
8664 final double sz = ma.getElementAt(2, 2);
8665 final double mxy = ma.getElementAt(0, 1);
8666 final double mxz = ma.getElementAt(0, 2);
8667 final double myx = ma.getElementAt(1, 0);
8668 final double myz = ma.getElementAt(1, 2);
8669 final double mzx = ma.getElementAt(2, 0);
8670 final double mzy = ma.getElementAt(2, 1);
8671
8672 KnownGravityNormAccelerometerCalibrator calibrator =
8673 new KnownGravityNormAccelerometerCalibrator(measurements,
8674 true, ba, ma);
8675
8676
8677 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8678 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8679 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8680 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8681 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8682 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8683 final Acceleration bx2 = new Acceleration(0.0,
8684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8685 calibrator.getInitialBiasXAsAcceleration(bx2);
8686 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8687 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8688 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8689 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8690 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8691 final Acceleration by2 = new Acceleration(0.0,
8692 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8693 calibrator.getInitialBiasYAsAcceleration(by2);
8694 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8695 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8696 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8697 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8698 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8699 final Acceleration bz2 = new Acceleration(0.0,
8700 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8701 calibrator.getInitialBiasZAsAcceleration(bz2);
8702 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8703 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8704 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8705 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8706 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8707 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8708 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8709 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8710 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8711 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8712 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8713 final double[] bias1 = calibrator.getInitialBias();
8714 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8715 final double[] bias2 = new double[3];
8716 calibrator.getInitialBias(bias2);
8717 assertArrayEquals(bias1, bias2, 0.0);
8718 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8719 assertEquals(b1, ba);
8720 final Matrix b2 = new Matrix(3, 1);
8721 calibrator.getInitialBiasAsMatrix(b2);
8722 assertEquals(b1, b2);
8723 final Matrix ma1 = new Matrix(3, 3);
8724 ma1.setSubmatrix(0, 0,
8725 2, 2,
8726 new double[]{sx, myx, mzx,
8727 mxy, sy, mzy,
8728 mxz, myz, sz});
8729 assertEquals(calibrator.getInitialMa(), ma1);
8730 final Matrix ma2 = new Matrix(3, 3);
8731 calibrator.getInitialMa(ma2);
8732 assertEquals(ma1, ma2);
8733 assertSame(calibrator.getMeasurements(), measurements);
8734 assertTrue(calibrator.isCommonAxisUsed());
8735 assertNull(calibrator.getListener());
8736 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8737 assertFalse(calibrator.isReady());
8738 assertFalse(calibrator.isRunning());
8739 assertNull(calibrator.getEstimatedBiases());
8740 assertFalse(calibrator.getEstimatedBiases(null));
8741 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8742 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8743 assertNull(calibrator.getEstimatedBiasFx());
8744 assertNull(calibrator.getEstimatedBiasFy());
8745 assertNull(calibrator.getEstimatedBiasFz());
8746 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8747 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8748 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8749 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8750 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8751 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8752 assertNull(calibrator.getEstimatedMa());
8753 assertNull(calibrator.getEstimatedSx());
8754 assertNull(calibrator.getEstimatedSy());
8755 assertNull(calibrator.getEstimatedSz());
8756 assertNull(calibrator.getEstimatedMxy());
8757 assertNull(calibrator.getEstimatedMxz());
8758 assertNull(calibrator.getEstimatedMyx());
8759 assertNull(calibrator.getEstimatedMyz());
8760 assertNull(calibrator.getEstimatedMzx());
8761 assertNull(calibrator.getEstimatedMzy());
8762 assertNull(calibrator.getEstimatedCovariance());
8763 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8764 assertNull(calibrator.getGroundTruthGravityNorm());
8765 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8766 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8767
8768
8769 calibrator = null;
8770 try {
8771 calibrator = new KnownGravityNormAccelerometerCalibrator(
8772 measurements, true,
8773 new Matrix(1, 1), ma);
8774 fail("IllegalArgumentException expected but not thrown");
8775 } catch (final IllegalArgumentException ignore) {
8776 }
8777 try {
8778 calibrator = new KnownGravityNormAccelerometerCalibrator(
8779 measurements, true,
8780 new Matrix(1, 3), ma);
8781 fail("IllegalArgumentException expected but not thrown");
8782 } catch (final IllegalArgumentException ignore) {
8783 }
8784 try {
8785 calibrator = new KnownGravityNormAccelerometerCalibrator(
8786 measurements, true, ba,
8787 new Matrix(1, 3));
8788 fail("IllegalArgumentException expected but not thrown");
8789 } catch (final IllegalArgumentException ignore) {
8790 }
8791 try {
8792 calibrator = new KnownGravityNormAccelerometerCalibrator(
8793 measurements, true, ba,
8794 new Matrix(3, 1));
8795 fail("IllegalArgumentException expected but not thrown");
8796 } catch (final IllegalArgumentException ignore) {
8797 }
8798 assertNull(calibrator);
8799 }
8800
8801 @Test
8802 public void testConstructor78() throws WrongSizeException {
8803 final Collection<StandardDeviationBodyKinematics> measurements =
8804 Collections.emptyList();
8805
8806 final Matrix ba = generateBa();
8807 final double biasX = ba.getElementAtIndex(0);
8808 final double biasY = ba.getElementAtIndex(1);
8809 final double biasZ = ba.getElementAtIndex(2);
8810
8811 final Matrix ma = generateMaCommonAxis();
8812 final double sx = ma.getElementAt(0, 0);
8813 final double sy = ma.getElementAt(1, 1);
8814 final double sz = ma.getElementAt(2, 2);
8815 final double mxy = ma.getElementAt(0, 1);
8816 final double mxz = ma.getElementAt(0, 2);
8817 final double myx = ma.getElementAt(1, 0);
8818 final double myz = ma.getElementAt(1, 2);
8819 final double mzx = ma.getElementAt(2, 0);
8820 final double mzy = ma.getElementAt(2, 1);
8821
8822 KnownGravityNormAccelerometerCalibrator calibrator =
8823 new KnownGravityNormAccelerometerCalibrator(measurements,
8824 true, ba, ma, this);
8825
8826
8827 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8828 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8829 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8830 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8831 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8832 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8833 final Acceleration bx2 = new Acceleration(0.0,
8834 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8835 calibrator.getInitialBiasXAsAcceleration(bx2);
8836 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8837 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8838 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8839 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8840 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8841 final Acceleration by2 = new Acceleration(0.0,
8842 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8843 calibrator.getInitialBiasYAsAcceleration(by2);
8844 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8845 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8846 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8847 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8848 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8849 final Acceleration bz2 = new Acceleration(0.0,
8850 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8851 calibrator.getInitialBiasZAsAcceleration(bz2);
8852 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8853 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8854 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8855 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8856 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8857 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8858 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8859 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8860 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8861 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8862 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8863 final double[] bias1 = calibrator.getInitialBias();
8864 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8865 final double[] bias2 = new double[3];
8866 calibrator.getInitialBias(bias2);
8867 assertArrayEquals(bias1, bias2, 0.0);
8868 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8869 assertEquals(b1, ba);
8870 final Matrix b2 = new Matrix(3, 1);
8871 calibrator.getInitialBiasAsMatrix(b2);
8872 assertEquals(b1, b2);
8873 final Matrix ma1 = new Matrix(3, 3);
8874 ma1.setSubmatrix(0, 0,
8875 2, 2,
8876 new double[]{sx, myx, mzx,
8877 mxy, sy, mzy,
8878 mxz, myz, sz});
8879 assertEquals(calibrator.getInitialMa(), ma1);
8880 final Matrix ma2 = new Matrix(3, 3);
8881 calibrator.getInitialMa(ma2);
8882 assertEquals(ma1, ma2);
8883 assertSame(calibrator.getMeasurements(), measurements);
8884 assertTrue(calibrator.isCommonAxisUsed());
8885 assertSame(calibrator.getListener(), this);
8886 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8887 assertFalse(calibrator.isReady());
8888 assertFalse(calibrator.isRunning());
8889 assertNull(calibrator.getEstimatedBiases());
8890 assertFalse(calibrator.getEstimatedBiases(null));
8891 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8892 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8893 assertNull(calibrator.getEstimatedBiasFx());
8894 assertNull(calibrator.getEstimatedBiasFy());
8895 assertNull(calibrator.getEstimatedBiasFz());
8896 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8897 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8898 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8899 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8900 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8901 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8902 assertNull(calibrator.getEstimatedMa());
8903 assertNull(calibrator.getEstimatedSx());
8904 assertNull(calibrator.getEstimatedSy());
8905 assertNull(calibrator.getEstimatedSz());
8906 assertNull(calibrator.getEstimatedMxy());
8907 assertNull(calibrator.getEstimatedMxz());
8908 assertNull(calibrator.getEstimatedMyx());
8909 assertNull(calibrator.getEstimatedMyz());
8910 assertNull(calibrator.getEstimatedMzx());
8911 assertNull(calibrator.getEstimatedMzy());
8912 assertNull(calibrator.getEstimatedCovariance());
8913 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8914 assertNull(calibrator.getGroundTruthGravityNorm());
8915 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8916 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8917
8918
8919 calibrator = null;
8920 try {
8921 calibrator = new KnownGravityNormAccelerometerCalibrator(
8922 measurements, true,
8923 new Matrix(1, 1), ma,
8924 this);
8925 fail("IllegalArgumentException expected but not thrown");
8926 } catch (final IllegalArgumentException ignore) {
8927 }
8928 try {
8929 calibrator = new KnownGravityNormAccelerometerCalibrator(
8930 measurements, true,
8931 new Matrix(1, 3), ma,
8932 this);
8933 fail("IllegalArgumentException expected but not thrown");
8934 } catch (final IllegalArgumentException ignore) {
8935 }
8936 try {
8937 calibrator = new KnownGravityNormAccelerometerCalibrator(
8938 measurements, true, ba, new Matrix(1, 3),
8939 this);
8940 fail("IllegalArgumentException expected but not thrown");
8941 } catch (final IllegalArgumentException ignore) {
8942 }
8943 try {
8944 calibrator = new KnownGravityNormAccelerometerCalibrator(
8945 measurements, true, ba, new Matrix(3, 1),
8946 this);
8947 fail("IllegalArgumentException expected but not thrown");
8948 } catch (final IllegalArgumentException ignore) {
8949 }
8950 assertNull(calibrator);
8951 }
8952
8953 @Test
8954 public void testConstructor79() throws WrongSizeException {
8955 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8956 final double latitude = Math.toRadians(
8957 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8958 final double longitude = Math.toRadians(
8959 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8960 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8961 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8962 final NEDVelocity nedVelocity = new NEDVelocity();
8963 final ECEFPosition ecefPosition = new ECEFPosition();
8964 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8965 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8966 ecefPosition, ecefVelocity);
8967 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8968 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8969 final double gravityNorm = gravity.getNorm();
8970
8971 KnownGravityNormAccelerometerCalibrator calibrator =
8972 new KnownGravityNormAccelerometerCalibrator(gravityNorm);
8973
8974
8975 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
8976 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
8977 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
8978 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8979 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8980 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8981 final Acceleration bx2 = new Acceleration(0.0,
8982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8983 calibrator.getInitialBiasXAsAcceleration(bx2);
8984 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8985 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8986 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8987 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8988 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8989 final Acceleration by2 = new Acceleration(0.0,
8990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8991 calibrator.getInitialBiasYAsAcceleration(by2);
8992 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8993 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8994 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8995 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8996 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8997 final Acceleration bz2 = new Acceleration(0.0,
8998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8999 calibrator.getInitialBiasZAsAcceleration(bz2);
9000 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9001 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9002 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9003 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9004 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9005 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9006 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9007 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9008 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9009 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9010 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9011 final double[] bias1 = calibrator.getInitialBias();
9012 assertArrayEquals(bias1, new double[3], 0.0);
9013 final double[] bias2 = new double[3];
9014 calibrator.getInitialBias(bias2);
9015 assertArrayEquals(bias1, bias2, 0.0);
9016 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9017 assertEquals(b1, new Matrix(3, 1));
9018 final Matrix b2 = new Matrix(3, 1);
9019 calibrator.getInitialBiasAsMatrix(b2);
9020 assertEquals(b1, b2);
9021 final Matrix ma1 = calibrator.getInitialMa();
9022 assertEquals(ma1, new Matrix(3, 3));
9023 final Matrix ma2 = new Matrix(3, 3);
9024 calibrator.getInitialMa(ma2);
9025 assertEquals(ma1, ma2);
9026 assertNull(calibrator.getMeasurements());
9027 assertFalse(calibrator.isCommonAxisUsed());
9028 assertNull(calibrator.getListener());
9029 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9030 assertFalse(calibrator.isReady());
9031 assertFalse(calibrator.isRunning());
9032 assertNull(calibrator.getEstimatedBiases());
9033 assertFalse(calibrator.getEstimatedBiases(null));
9034 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9035 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9036 assertNull(calibrator.getEstimatedBiasFx());
9037 assertNull(calibrator.getEstimatedBiasFy());
9038 assertNull(calibrator.getEstimatedBiasFz());
9039 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9040 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9041 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9042 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9043 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9044 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9045 assertNull(calibrator.getEstimatedMa());
9046 assertNull(calibrator.getEstimatedSx());
9047 assertNull(calibrator.getEstimatedSy());
9048 assertNull(calibrator.getEstimatedSz());
9049 assertNull(calibrator.getEstimatedMxy());
9050 assertNull(calibrator.getEstimatedMxz());
9051 assertNull(calibrator.getEstimatedMyx());
9052 assertNull(calibrator.getEstimatedMyz());
9053 assertNull(calibrator.getEstimatedMzx());
9054 assertNull(calibrator.getEstimatedMzy());
9055 assertNull(calibrator.getEstimatedCovariance());
9056 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9057 assertNotNull(calibrator.getGroundTruthGravityNorm());
9058 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9059 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9060 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9061 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9062 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9063 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9064 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9065
9066
9067 calibrator = null;
9068 try {
9069 calibrator = new KnownGravityNormAccelerometerCalibrator(
9070 -gravityNorm);
9071 fail("IllegalArgumentException expected but not thrown");
9072 } catch (final IllegalArgumentException ignore) {
9073 }
9074 assertNull(calibrator);
9075 }
9076
9077 @Test
9078 public void testConstructor80() throws WrongSizeException {
9079 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9080 final double latitude = Math.toRadians(
9081 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9082 final double longitude = Math.toRadians(
9083 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9084 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9085 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9086 final NEDVelocity nedVelocity = new NEDVelocity();
9087 final ECEFPosition ecefPosition = new ECEFPosition();
9088 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9089 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9090 ecefPosition, ecefVelocity);
9091 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9092 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9093 final double gravityNorm = gravity.getNorm();
9094
9095 KnownGravityNormAccelerometerCalibrator calibrator =
9096 new KnownGravityNormAccelerometerCalibrator(
9097 gravityNorm, this);
9098
9099
9100 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9101 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9102 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9103 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9104 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9105 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9106 final Acceleration bx2 = new Acceleration(0.0,
9107 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9108 calibrator.getInitialBiasXAsAcceleration(bx2);
9109 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9110 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9111 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9112 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9113 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9114 final Acceleration by2 = new Acceleration(0.0,
9115 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9116 calibrator.getInitialBiasYAsAcceleration(by2);
9117 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9118 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9119 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9120 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9121 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9122 final Acceleration bz2 = new Acceleration(0.0,
9123 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9124 calibrator.getInitialBiasZAsAcceleration(bz2);
9125 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9126 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9127 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9128 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9129 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9130 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9131 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9132 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9133 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9134 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9135 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9136 final double[] bias1 = calibrator.getInitialBias();
9137 assertArrayEquals(bias1, new double[3], 0.0);
9138 final double[] bias2 = new double[3];
9139 calibrator.getInitialBias(bias2);
9140 assertArrayEquals(bias1, bias2, 0.0);
9141 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9142 assertEquals(b1, new Matrix(3, 1));
9143 final Matrix b2 = new Matrix(3, 1);
9144 calibrator.getInitialBiasAsMatrix(b2);
9145 assertEquals(b1, b2);
9146 final Matrix ma1 = calibrator.getInitialMa();
9147 assertEquals(ma1, new Matrix(3, 3));
9148 final Matrix ma2 = new Matrix(3, 3);
9149 calibrator.getInitialMa(ma2);
9150 assertEquals(ma1, ma2);
9151 assertNull(calibrator.getMeasurements());
9152 assertFalse(calibrator.isCommonAxisUsed());
9153 assertSame(calibrator.getListener(), this);
9154 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9155 assertFalse(calibrator.isReady());
9156 assertFalse(calibrator.isRunning());
9157 assertNull(calibrator.getEstimatedBiases());
9158 assertFalse(calibrator.getEstimatedBiases(null));
9159 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9160 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9161 assertNull(calibrator.getEstimatedBiasFx());
9162 assertNull(calibrator.getEstimatedBiasFy());
9163 assertNull(calibrator.getEstimatedBiasFz());
9164 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9165 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9166 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9167 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9168 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9169 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9170 assertNull(calibrator.getEstimatedMa());
9171 assertNull(calibrator.getEstimatedSx());
9172 assertNull(calibrator.getEstimatedSy());
9173 assertNull(calibrator.getEstimatedSz());
9174 assertNull(calibrator.getEstimatedMxy());
9175 assertNull(calibrator.getEstimatedMxz());
9176 assertNull(calibrator.getEstimatedMyx());
9177 assertNull(calibrator.getEstimatedMyz());
9178 assertNull(calibrator.getEstimatedMzx());
9179 assertNull(calibrator.getEstimatedMzy());
9180 assertNull(calibrator.getEstimatedCovariance());
9181 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9182 assertNotNull(calibrator.getGroundTruthGravityNorm());
9183 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9184 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9185 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9186 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9187 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9188 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9189 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9190
9191
9192 calibrator = null;
9193 try {
9194 calibrator = new KnownGravityNormAccelerometerCalibrator(
9195 -gravityNorm, this);
9196 fail("IllegalArgumentException expected but not thrown");
9197 } catch (final IllegalArgumentException ignore) {
9198 }
9199 assertNull(calibrator);
9200 }
9201
9202 @Test
9203 public void testConstructor81() throws WrongSizeException {
9204 final Collection<StandardDeviationBodyKinematics> measurements =
9205 Collections.emptyList();
9206
9207 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9208 final double latitude = Math.toRadians(
9209 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9210 final double longitude = Math.toRadians(
9211 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9212 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9213 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9214 final NEDVelocity nedVelocity = new NEDVelocity();
9215 final ECEFPosition ecefPosition = new ECEFPosition();
9216 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9217 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9218 ecefPosition, ecefVelocity);
9219 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9220 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9221 final double gravityNorm = gravity.getNorm();
9222
9223 KnownGravityNormAccelerometerCalibrator calibrator =
9224 new KnownGravityNormAccelerometerCalibrator(
9225 gravityNorm, measurements);
9226
9227
9228 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9229 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9230 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9231 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9232 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9233 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9234 final Acceleration bx2 = new Acceleration(0.0,
9235 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9236 calibrator.getInitialBiasXAsAcceleration(bx2);
9237 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9238 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9239 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9240 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9241 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9242 final Acceleration by2 = new Acceleration(0.0,
9243 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9244 calibrator.getInitialBiasYAsAcceleration(by2);
9245 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9246 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9247 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9248 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9249 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9250 final Acceleration bz2 = new Acceleration(0.0,
9251 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9252 calibrator.getInitialBiasZAsAcceleration(bz2);
9253 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9254 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9255 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9256 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9257 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9258 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9259 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9260 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9261 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9262 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9263 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9264 final double[] bias1 = calibrator.getInitialBias();
9265 assertArrayEquals(bias1, new double[3], 0.0);
9266 final double[] bias2 = new double[3];
9267 calibrator.getInitialBias(bias2);
9268 assertArrayEquals(bias1, bias2, 0.0);
9269 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9270 assertEquals(b1, new Matrix(3, 1));
9271 final Matrix b2 = new Matrix(3, 1);
9272 calibrator.getInitialBiasAsMatrix(b2);
9273 assertEquals(b1, b2);
9274 final Matrix ma1 = calibrator.getInitialMa();
9275 assertEquals(ma1, new Matrix(3, 3));
9276 final Matrix ma2 = new Matrix(3, 3);
9277 calibrator.getInitialMa(ma2);
9278 assertEquals(ma1, ma2);
9279 assertSame(calibrator.getMeasurements(), measurements);
9280 assertFalse(calibrator.isCommonAxisUsed());
9281 assertNull(calibrator.getListener());
9282 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9283 assertFalse(calibrator.isReady());
9284 assertFalse(calibrator.isRunning());
9285 assertNull(calibrator.getEstimatedBiases());
9286 assertFalse(calibrator.getEstimatedBiases(null));
9287 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9288 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9289 assertNull(calibrator.getEstimatedBiasFx());
9290 assertNull(calibrator.getEstimatedBiasFy());
9291 assertNull(calibrator.getEstimatedBiasFz());
9292 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9293 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9294 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9295 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9296 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9297 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9298 assertNull(calibrator.getEstimatedMa());
9299 assertNull(calibrator.getEstimatedSx());
9300 assertNull(calibrator.getEstimatedSy());
9301 assertNull(calibrator.getEstimatedSz());
9302 assertNull(calibrator.getEstimatedMxy());
9303 assertNull(calibrator.getEstimatedMxz());
9304 assertNull(calibrator.getEstimatedMyx());
9305 assertNull(calibrator.getEstimatedMyz());
9306 assertNull(calibrator.getEstimatedMzx());
9307 assertNull(calibrator.getEstimatedMzy());
9308 assertNull(calibrator.getEstimatedCovariance());
9309 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9310 assertNotNull(calibrator.getGroundTruthGravityNorm());
9311 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9312 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9313 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9314 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9315 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9316 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9317 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9318
9319
9320 calibrator = null;
9321 try {
9322 calibrator = new KnownGravityNormAccelerometerCalibrator(
9323 -gravityNorm, measurements);
9324 fail("IllegalArgumentException expected but not thrown");
9325 } catch (final IllegalArgumentException ignore) {
9326 }
9327 assertNull(calibrator);
9328 }
9329
9330 @Test
9331 public void testConstructor82() throws WrongSizeException {
9332 final Collection<StandardDeviationBodyKinematics> measurements =
9333 Collections.emptyList();
9334
9335 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9336 final double latitude = Math.toRadians(
9337 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9338 final double longitude = Math.toRadians(
9339 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9340 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9341 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9342 final NEDVelocity nedVelocity = new NEDVelocity();
9343 final ECEFPosition ecefPosition = new ECEFPosition();
9344 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9345 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9346 ecefPosition, ecefVelocity);
9347 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9348 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9349 final double gravityNorm = gravity.getNorm();
9350
9351 KnownGravityNormAccelerometerCalibrator calibrator =
9352 new KnownGravityNormAccelerometerCalibrator(
9353 gravityNorm, measurements, this);
9354
9355
9356 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9357 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9358 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9359 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9360 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9361 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9362 final Acceleration bx2 = new Acceleration(0.0,
9363 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9364 calibrator.getInitialBiasXAsAcceleration(bx2);
9365 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9366 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9367 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9368 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9369 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9370 final Acceleration by2 = new Acceleration(0.0,
9371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9372 calibrator.getInitialBiasYAsAcceleration(by2);
9373 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9374 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9375 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9376 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9377 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9378 final Acceleration bz2 = new Acceleration(0.0,
9379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9380 calibrator.getInitialBiasZAsAcceleration(bz2);
9381 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9382 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9383 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9384 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9385 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9386 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9387 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9388 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9389 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9390 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9391 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9392 final double[] bias1 = calibrator.getInitialBias();
9393 assertArrayEquals(bias1, new double[3], 0.0);
9394 final double[] bias2 = new double[3];
9395 calibrator.getInitialBias(bias2);
9396 assertArrayEquals(bias1, bias2, 0.0);
9397 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9398 assertEquals(b1, new Matrix(3, 1));
9399 final Matrix b2 = new Matrix(3, 1);
9400 calibrator.getInitialBiasAsMatrix(b2);
9401 assertEquals(b1, b2);
9402 final Matrix ma1 = calibrator.getInitialMa();
9403 assertEquals(ma1, new Matrix(3, 3));
9404 final Matrix ma2 = new Matrix(3, 3);
9405 calibrator.getInitialMa(ma2);
9406 assertEquals(ma1, ma2);
9407 assertSame(calibrator.getMeasurements(), measurements);
9408 assertFalse(calibrator.isCommonAxisUsed());
9409 assertSame(calibrator.getListener(), this);
9410 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9411 assertFalse(calibrator.isReady());
9412 assertFalse(calibrator.isRunning());
9413 assertNull(calibrator.getEstimatedBiases());
9414 assertFalse(calibrator.getEstimatedBiases(null));
9415 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9416 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9417 assertNull(calibrator.getEstimatedBiasFx());
9418 assertNull(calibrator.getEstimatedBiasFy());
9419 assertNull(calibrator.getEstimatedBiasFz());
9420 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9421 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9422 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9423 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9424 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9425 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9426 assertNull(calibrator.getEstimatedMa());
9427 assertNull(calibrator.getEstimatedSx());
9428 assertNull(calibrator.getEstimatedSy());
9429 assertNull(calibrator.getEstimatedSz());
9430 assertNull(calibrator.getEstimatedMxy());
9431 assertNull(calibrator.getEstimatedMxz());
9432 assertNull(calibrator.getEstimatedMyx());
9433 assertNull(calibrator.getEstimatedMyz());
9434 assertNull(calibrator.getEstimatedMzx());
9435 assertNull(calibrator.getEstimatedMzy());
9436 assertNull(calibrator.getEstimatedCovariance());
9437 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9438 assertNotNull(calibrator.getGroundTruthGravityNorm());
9439 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9440 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9441 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9442 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9443 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9444 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9445 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9446
9447
9448 calibrator = null;
9449 try {
9450 calibrator = new KnownGravityNormAccelerometerCalibrator(
9451 -gravityNorm, measurements, this);
9452 fail("IllegalArgumentException expected but not thrown");
9453 } catch (final IllegalArgumentException ignore) {
9454 }
9455 assertNull(calibrator);
9456 }
9457
9458 @Test
9459 public void testConstructor83() throws WrongSizeException {
9460 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9461 final double latitude = Math.toRadians(
9462 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9463 final double longitude = Math.toRadians(
9464 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9465 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9466 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9467 final NEDVelocity nedVelocity = new NEDVelocity();
9468 final ECEFPosition ecefPosition = new ECEFPosition();
9469 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9470 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9471 ecefPosition, ecefVelocity);
9472 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9473 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9474 final double gravityNorm = gravity.getNorm();
9475
9476 KnownGravityNormAccelerometerCalibrator calibrator =
9477 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9478 true);
9479
9480
9481 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9482 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9483 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9484 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9485 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9486 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9487 final Acceleration bx2 = new Acceleration(0.0,
9488 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9489 calibrator.getInitialBiasXAsAcceleration(bx2);
9490 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9491 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9492 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9493 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9494 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9495 final Acceleration by2 = new Acceleration(0.0,
9496 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9497 calibrator.getInitialBiasYAsAcceleration(by2);
9498 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9499 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9500 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9501 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9502 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9503 final Acceleration bz2 = new Acceleration(0.0,
9504 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9505 calibrator.getInitialBiasZAsAcceleration(bz2);
9506 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9507 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9508 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9509 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9510 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9511 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9512 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9513 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9514 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9515 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9516 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9517 final double[] bias1 = calibrator.getInitialBias();
9518 assertArrayEquals(bias1, new double[3], 0.0);
9519 final double[] bias2 = new double[3];
9520 calibrator.getInitialBias(bias2);
9521 assertArrayEquals(bias1, bias2, 0.0);
9522 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9523 assertEquals(b1, new Matrix(3, 1));
9524 final Matrix b2 = new Matrix(3, 1);
9525 calibrator.getInitialBiasAsMatrix(b2);
9526 assertEquals(b1, b2);
9527 final Matrix ma1 = calibrator.getInitialMa();
9528 assertEquals(ma1, new Matrix(3, 3));
9529 final Matrix ma2 = new Matrix(3, 3);
9530 calibrator.getInitialMa(ma2);
9531 assertEquals(ma1, ma2);
9532 assertNull(calibrator.getMeasurements());
9533 assertTrue(calibrator.isCommonAxisUsed());
9534 assertNull(calibrator.getListener());
9535 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9536 assertFalse(calibrator.isReady());
9537 assertFalse(calibrator.isRunning());
9538 assertNull(calibrator.getEstimatedBiases());
9539 assertFalse(calibrator.getEstimatedBiases(null));
9540 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9541 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9542 assertNull(calibrator.getEstimatedBiasFx());
9543 assertNull(calibrator.getEstimatedBiasFy());
9544 assertNull(calibrator.getEstimatedBiasFz());
9545 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9546 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9547 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9548 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9549 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9550 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9551 assertNull(calibrator.getEstimatedMa());
9552 assertNull(calibrator.getEstimatedSx());
9553 assertNull(calibrator.getEstimatedSy());
9554 assertNull(calibrator.getEstimatedSz());
9555 assertNull(calibrator.getEstimatedMxy());
9556 assertNull(calibrator.getEstimatedMxz());
9557 assertNull(calibrator.getEstimatedMyx());
9558 assertNull(calibrator.getEstimatedMyz());
9559 assertNull(calibrator.getEstimatedMzx());
9560 assertNull(calibrator.getEstimatedMzy());
9561 assertNull(calibrator.getEstimatedCovariance());
9562 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9563 assertNotNull(calibrator.getGroundTruthGravityNorm());
9564 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9565 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9566 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9567 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9568 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9569 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9570 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9571
9572
9573 calibrator = null;
9574 try {
9575 calibrator = new KnownGravityNormAccelerometerCalibrator(
9576 -gravityNorm, true);
9577 fail("IllegalArgumentException expected but not thrown");
9578 } catch (final IllegalArgumentException ignore) {
9579 }
9580 assertNull(calibrator);
9581 }
9582
9583 @Test
9584 public void testConstructor84() throws WrongSizeException {
9585 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9586 final double latitude = Math.toRadians(
9587 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9588 final double longitude = Math.toRadians(
9589 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9590 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9591 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9592 final NEDVelocity nedVelocity = new NEDVelocity();
9593 final ECEFPosition ecefPosition = new ECEFPosition();
9594 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9595 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9596 ecefPosition, ecefVelocity);
9597 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9598 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9599 final double gravityNorm = gravity.getNorm();
9600
9601 KnownGravityNormAccelerometerCalibrator calibrator =
9602 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9603 true, this);
9604
9605
9606 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9607 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9608 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9609 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9610 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9611 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9612 final Acceleration bx2 = new Acceleration(0.0,
9613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9614 calibrator.getInitialBiasXAsAcceleration(bx2);
9615 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9616 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9617 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9618 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9619 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9620 final Acceleration by2 = new Acceleration(0.0,
9621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9622 calibrator.getInitialBiasYAsAcceleration(by2);
9623 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9624 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9625 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9626 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9627 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9628 final Acceleration bz2 = new Acceleration(0.0,
9629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9630 calibrator.getInitialBiasZAsAcceleration(bz2);
9631 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9632 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9633 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9634 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9635 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9636 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9637 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9638 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9639 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9640 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9641 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9642 final double[] bias1 = calibrator.getInitialBias();
9643 assertArrayEquals(bias1, new double[3], 0.0);
9644 final double[] bias2 = new double[3];
9645 calibrator.getInitialBias(bias2);
9646 assertArrayEquals(bias1, bias2, 0.0);
9647 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9648 assertEquals(b1, new Matrix(3, 1));
9649 final Matrix b2 = new Matrix(3, 1);
9650 calibrator.getInitialBiasAsMatrix(b2);
9651 assertEquals(b1, b2);
9652 final Matrix ma1 = calibrator.getInitialMa();
9653 assertEquals(ma1, new Matrix(3, 3));
9654 final Matrix ma2 = new Matrix(3, 3);
9655 calibrator.getInitialMa(ma2);
9656 assertEquals(ma1, ma2);
9657 assertNull(calibrator.getMeasurements());
9658 assertTrue(calibrator.isCommonAxisUsed());
9659 assertSame(calibrator.getListener(), this);
9660 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9661 assertFalse(calibrator.isReady());
9662 assertFalse(calibrator.isRunning());
9663 assertNull(calibrator.getEstimatedBiases());
9664 assertFalse(calibrator.getEstimatedBiases(null));
9665 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9666 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9667 assertNull(calibrator.getEstimatedBiasFx());
9668 assertNull(calibrator.getEstimatedBiasFy());
9669 assertNull(calibrator.getEstimatedBiasFz());
9670 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9671 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9672 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9673 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9674 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9675 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9676 assertNull(calibrator.getEstimatedMa());
9677 assertNull(calibrator.getEstimatedSx());
9678 assertNull(calibrator.getEstimatedSy());
9679 assertNull(calibrator.getEstimatedSz());
9680 assertNull(calibrator.getEstimatedMxy());
9681 assertNull(calibrator.getEstimatedMxz());
9682 assertNull(calibrator.getEstimatedMyx());
9683 assertNull(calibrator.getEstimatedMyz());
9684 assertNull(calibrator.getEstimatedMzx());
9685 assertNull(calibrator.getEstimatedMzy());
9686 assertNull(calibrator.getEstimatedCovariance());
9687 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9688 assertNotNull(calibrator.getGroundTruthGravityNorm());
9689 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9690 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9691 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9692 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9693 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9694 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9695 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9696
9697
9698 calibrator = null;
9699 try {
9700 calibrator = new KnownGravityNormAccelerometerCalibrator(
9701 -gravityNorm, true, this);
9702 fail("IllegalArgumentException expected but not thrown");
9703 } catch (final IllegalArgumentException ignore) {
9704 }
9705 assertNull(calibrator);
9706 }
9707
9708 @Test
9709 public void testConstructor85() throws WrongSizeException {
9710 final Collection<StandardDeviationBodyKinematics> measurements =
9711 Collections.emptyList();
9712 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9713 final double latitude = Math.toRadians(
9714 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9715 final double longitude = Math.toRadians(
9716 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9717 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9718 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9719 final NEDVelocity nedVelocity = new NEDVelocity();
9720 final ECEFPosition ecefPosition = new ECEFPosition();
9721 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9722 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9723 ecefPosition, ecefVelocity);
9724 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9725 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9726 final double gravityNorm = gravity.getNorm();
9727
9728 KnownGravityNormAccelerometerCalibrator calibrator =
9729 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9730 measurements, true);
9731
9732
9733 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9734 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9735 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9736 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9737 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9738 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9739 final Acceleration bx2 = new Acceleration(0.0,
9740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9741 calibrator.getInitialBiasXAsAcceleration(bx2);
9742 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9743 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9744 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9745 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9746 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9747 final Acceleration by2 = new Acceleration(0.0,
9748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9749 calibrator.getInitialBiasYAsAcceleration(by2);
9750 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9751 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9752 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9753 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9754 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9755 final Acceleration bz2 = new Acceleration(0.0,
9756 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9757 calibrator.getInitialBiasZAsAcceleration(bz2);
9758 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9759 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9760 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9761 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9762 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9763 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9764 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9765 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9766 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9767 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9768 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9769 final double[] bias1 = calibrator.getInitialBias();
9770 assertArrayEquals(bias1, new double[3], 0.0);
9771 final double[] bias2 = new double[3];
9772 calibrator.getInitialBias(bias2);
9773 assertArrayEquals(bias1, bias2, 0.0);
9774 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9775 assertEquals(b1, new Matrix(3, 1));
9776 final Matrix b2 = new Matrix(3, 1);
9777 calibrator.getInitialBiasAsMatrix(b2);
9778 assertEquals(b1, b2);
9779 final Matrix ma1 = calibrator.getInitialMa();
9780 assertEquals(ma1, new Matrix(3, 3));
9781 final Matrix ma2 = new Matrix(3, 3);
9782 calibrator.getInitialMa(ma2);
9783 assertEquals(ma1, ma2);
9784 assertSame(calibrator.getMeasurements(), measurements);
9785 assertTrue(calibrator.isCommonAxisUsed());
9786 assertNull(calibrator.getListener());
9787 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9788 assertFalse(calibrator.isReady());
9789 assertFalse(calibrator.isRunning());
9790 assertNull(calibrator.getEstimatedBiases());
9791 assertFalse(calibrator.getEstimatedBiases(null));
9792 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9793 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9794 assertNull(calibrator.getEstimatedBiasFx());
9795 assertNull(calibrator.getEstimatedBiasFy());
9796 assertNull(calibrator.getEstimatedBiasFz());
9797 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9798 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9799 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9800 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9801 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9802 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9803 assertNull(calibrator.getEstimatedMa());
9804 assertNull(calibrator.getEstimatedSx());
9805 assertNull(calibrator.getEstimatedSy());
9806 assertNull(calibrator.getEstimatedSz());
9807 assertNull(calibrator.getEstimatedMxy());
9808 assertNull(calibrator.getEstimatedMxz());
9809 assertNull(calibrator.getEstimatedMyx());
9810 assertNull(calibrator.getEstimatedMyz());
9811 assertNull(calibrator.getEstimatedMzx());
9812 assertNull(calibrator.getEstimatedMzy());
9813 assertNull(calibrator.getEstimatedCovariance());
9814 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9815 assertNotNull(calibrator.getGroundTruthGravityNorm());
9816 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9817 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9818 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9819 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9820 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9821 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9822 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9823
9824
9825 calibrator = null;
9826 try {
9827 calibrator = new KnownGravityNormAccelerometerCalibrator(
9828 -gravityNorm, measurements, true);
9829 fail("IllegalArgumentException expected but not thrown");
9830 } catch (final IllegalArgumentException ignore) {
9831 }
9832 assertNull(calibrator);
9833 }
9834
9835 @Test
9836 public void testConstructor86() throws WrongSizeException {
9837 final Collection<StandardDeviationBodyKinematics> measurements =
9838 Collections.emptyList();
9839 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9840 final double latitude = Math.toRadians(
9841 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9842 final double longitude = Math.toRadians(
9843 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9844 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9845 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9846 final NEDVelocity nedVelocity = new NEDVelocity();
9847 final ECEFPosition ecefPosition = new ECEFPosition();
9848 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9849 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9850 ecefPosition, ecefVelocity);
9851 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9852 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9853 final double gravityNorm = gravity.getNorm();
9854
9855 KnownGravityNormAccelerometerCalibrator calibrator =
9856 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9857 measurements, true, this);
9858
9859
9860 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9861 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9862 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9863 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9864 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9865 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9866 final Acceleration bx2 = new Acceleration(0.0,
9867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9868 calibrator.getInitialBiasXAsAcceleration(bx2);
9869 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9870 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9871 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9872 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9873 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9874 final Acceleration by2 = new Acceleration(0.0,
9875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9876 calibrator.getInitialBiasYAsAcceleration(by2);
9877 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9878 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9879 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9880 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9881 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9882 final Acceleration bz2 = new Acceleration(0.0,
9883 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9884 calibrator.getInitialBiasZAsAcceleration(bz2);
9885 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9886 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9887 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9888 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9889 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9890 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9891 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9892 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9893 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9894 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9895 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9896 final double[] bias1 = calibrator.getInitialBias();
9897 assertArrayEquals(bias1, new double[3], 0.0);
9898 final double[] bias2 = new double[3];
9899 calibrator.getInitialBias(bias2);
9900 assertArrayEquals(bias1, bias2, 0.0);
9901 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9902 assertEquals(b1, new Matrix(3, 1));
9903 final Matrix b2 = new Matrix(3, 1);
9904 calibrator.getInitialBiasAsMatrix(b2);
9905 assertEquals(b1, b2);
9906 final Matrix ma1 = calibrator.getInitialMa();
9907 assertEquals(ma1, new Matrix(3, 3));
9908 final Matrix ma2 = new Matrix(3, 3);
9909 calibrator.getInitialMa(ma2);
9910 assertEquals(ma1, ma2);
9911 assertSame(calibrator.getMeasurements(), measurements);
9912 assertTrue(calibrator.isCommonAxisUsed());
9913 assertSame(calibrator.getListener(), this);
9914 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9915 assertFalse(calibrator.isReady());
9916 assertFalse(calibrator.isRunning());
9917 assertNull(calibrator.getEstimatedBiases());
9918 assertFalse(calibrator.getEstimatedBiases(null));
9919 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9920 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9921 assertNull(calibrator.getEstimatedBiasFx());
9922 assertNull(calibrator.getEstimatedBiasFy());
9923 assertNull(calibrator.getEstimatedBiasFz());
9924 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9925 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9926 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9927 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9928 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9929 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9930 assertNull(calibrator.getEstimatedMa());
9931 assertNull(calibrator.getEstimatedSx());
9932 assertNull(calibrator.getEstimatedSy());
9933 assertNull(calibrator.getEstimatedSz());
9934 assertNull(calibrator.getEstimatedMxy());
9935 assertNull(calibrator.getEstimatedMxz());
9936 assertNull(calibrator.getEstimatedMyx());
9937 assertNull(calibrator.getEstimatedMyz());
9938 assertNull(calibrator.getEstimatedMzx());
9939 assertNull(calibrator.getEstimatedMzy());
9940 assertNull(calibrator.getEstimatedCovariance());
9941 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9942 assertNotNull(calibrator.getGroundTruthGravityNorm());
9943 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9944 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9945 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9946 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9947 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9948 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9949 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9950
9951
9952 calibrator = null;
9953 try {
9954 calibrator = new KnownGravityNormAccelerometerCalibrator(
9955 -gravityNorm, measurements, true, this);
9956 fail("IllegalArgumentException expected but not thrown");
9957 } catch (final IllegalArgumentException ignore) {
9958 }
9959 assertNull(calibrator);
9960 }
9961
9962 @Test
9963 public void testConstructor87() throws WrongSizeException {
9964 final Matrix ba = generateBa();
9965 final double biasX = ba.getElementAtIndex(0);
9966 final double biasY = ba.getElementAtIndex(1);
9967 final double biasZ = ba.getElementAtIndex(2);
9968
9969 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9970 final double latitude = Math.toRadians(
9971 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9972 final double longitude = Math.toRadians(
9973 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9974 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9975 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9976 final NEDVelocity nedVelocity = new NEDVelocity();
9977 final ECEFPosition ecefPosition = new ECEFPosition();
9978 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9979 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9980 ecefPosition, ecefVelocity);
9981 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9982 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9983 final double gravityNorm = gravity.getNorm();
9984
9985 KnownGravityNormAccelerometerCalibrator calibrator =
9986 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9987 biasX, biasY, biasZ);
9988
9989
9990 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9991 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9992 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9993 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9994 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9995 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9996 final Acceleration bx2 = new Acceleration(0.0,
9997 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9998 calibrator.getInitialBiasXAsAcceleration(bx2);
9999 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10000 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10001 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10002 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10003 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10004 final Acceleration by2 = new Acceleration(0.0,
10005 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10006 calibrator.getInitialBiasYAsAcceleration(by2);
10007 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10008 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10009 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10010 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10011 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10012 final Acceleration bz2 = new Acceleration(0.0,
10013 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10014 calibrator.getInitialBiasZAsAcceleration(bz2);
10015 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10016 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10017 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10018 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10019 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10020 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10021 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10022 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10023 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10024 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10025 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10026 final double[] bias1 = calibrator.getInitialBias();
10027 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10028 final double[] bias2 = new double[3];
10029 calibrator.getInitialBias(bias2);
10030 assertArrayEquals(bias1, bias2, 0.0);
10031 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10032 assertEquals(b1, ba);
10033 final Matrix b2 = new Matrix(3, 1);
10034 calibrator.getInitialBiasAsMatrix(b2);
10035 assertEquals(b1, b2);
10036 final Matrix ma1 = calibrator.getInitialMa();
10037 assertEquals(ma1, new Matrix(3, 3));
10038 final Matrix ma2 = new Matrix(3, 3);
10039 calibrator.getInitialMa(ma2);
10040 assertEquals(ma1, ma2);
10041 assertNull(calibrator.getMeasurements());
10042 assertFalse(calibrator.isCommonAxisUsed());
10043 assertNull(calibrator.getListener());
10044 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10045 assertFalse(calibrator.isReady());
10046 assertFalse(calibrator.isRunning());
10047 assertNull(calibrator.getEstimatedBiases());
10048 assertFalse(calibrator.getEstimatedBiases(null));
10049 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10050 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10051 assertNull(calibrator.getEstimatedBiasFx());
10052 assertNull(calibrator.getEstimatedBiasFy());
10053 assertNull(calibrator.getEstimatedBiasFz());
10054 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10055 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10056 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10057 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10058 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10059 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10060 assertNull(calibrator.getEstimatedMa());
10061 assertNull(calibrator.getEstimatedSx());
10062 assertNull(calibrator.getEstimatedSy());
10063 assertNull(calibrator.getEstimatedSz());
10064 assertNull(calibrator.getEstimatedMxy());
10065 assertNull(calibrator.getEstimatedMxz());
10066 assertNull(calibrator.getEstimatedMyx());
10067 assertNull(calibrator.getEstimatedMyz());
10068 assertNull(calibrator.getEstimatedMzx());
10069 assertNull(calibrator.getEstimatedMzy());
10070 assertNull(calibrator.getEstimatedCovariance());
10071 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10072 assertNotNull(calibrator.getGroundTruthGravityNorm());
10073 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10074 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10075 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10076 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10077 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10078 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10079 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10080
10081
10082 calibrator = null;
10083 try {
10084 calibrator = new KnownGravityNormAccelerometerCalibrator(
10085 -gravityNorm, biasX, biasY, biasZ);
10086 fail("IllegalArgumentException expected but not thrown");
10087 } catch (final IllegalArgumentException ignore) {
10088 }
10089 assertNull(calibrator);
10090 }
10091
10092 @Test
10093 public void testConstructor88() throws WrongSizeException {
10094 final Matrix ba = generateBa();
10095 final double biasX = ba.getElementAtIndex(0);
10096 final double biasY = ba.getElementAtIndex(1);
10097 final double biasZ = ba.getElementAtIndex(2);
10098
10099 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10100 final double latitude = Math.toRadians(
10101 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10102 final double longitude = Math.toRadians(
10103 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10104 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10105 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10106 final NEDVelocity nedVelocity = new NEDVelocity();
10107 final ECEFPosition ecefPosition = new ECEFPosition();
10108 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10109 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10110 ecefPosition, ecefVelocity);
10111 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10112 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10113 final double gravityNorm = gravity.getNorm();
10114
10115 KnownGravityNormAccelerometerCalibrator calibrator =
10116 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10117 biasX, biasY, biasZ, this);
10118
10119
10120 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10121 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10122 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10123 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10124 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10125 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10126 final Acceleration bx2 = new Acceleration(0.0,
10127 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10128 calibrator.getInitialBiasXAsAcceleration(bx2);
10129 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10130 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10131 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10132 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10133 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10134 final Acceleration by2 = new Acceleration(0.0,
10135 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10136 calibrator.getInitialBiasYAsAcceleration(by2);
10137 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10138 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10139 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10140 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10141 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10142 final Acceleration bz2 = new Acceleration(0.0,
10143 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10144 calibrator.getInitialBiasZAsAcceleration(bz2);
10145 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10146 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10147 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10148 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10149 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10150 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10151 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10152 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10153 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10154 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10155 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10156 final double[] bias1 = calibrator.getInitialBias();
10157 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10158 final double[] bias2 = new double[3];
10159 calibrator.getInitialBias(bias2);
10160 assertArrayEquals(bias1, bias2, 0.0);
10161 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10162 assertEquals(b1, ba);
10163 final Matrix b2 = new Matrix(3, 1);
10164 calibrator.getInitialBiasAsMatrix(b2);
10165 assertEquals(b1, b2);
10166 final Matrix ma1 = calibrator.getInitialMa();
10167 assertEquals(ma1, new Matrix(3, 3));
10168 final Matrix ma2 = new Matrix(3, 3);
10169 calibrator.getInitialMa(ma2);
10170 assertEquals(ma1, ma2);
10171 assertNull(calibrator.getMeasurements());
10172 assertFalse(calibrator.isCommonAxisUsed());
10173 assertSame(calibrator.getListener(), this);
10174 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10175 assertFalse(calibrator.isReady());
10176 assertFalse(calibrator.isRunning());
10177 assertNull(calibrator.getEstimatedBiases());
10178 assertFalse(calibrator.getEstimatedBiases(null));
10179 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10180 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10181 assertNull(calibrator.getEstimatedBiasFx());
10182 assertNull(calibrator.getEstimatedBiasFy());
10183 assertNull(calibrator.getEstimatedBiasFz());
10184 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10185 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10186 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10187 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10188 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10189 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10190 assertNull(calibrator.getEstimatedMa());
10191 assertNull(calibrator.getEstimatedSx());
10192 assertNull(calibrator.getEstimatedSy());
10193 assertNull(calibrator.getEstimatedSz());
10194 assertNull(calibrator.getEstimatedMxy());
10195 assertNull(calibrator.getEstimatedMxz());
10196 assertNull(calibrator.getEstimatedMyx());
10197 assertNull(calibrator.getEstimatedMyz());
10198 assertNull(calibrator.getEstimatedMzx());
10199 assertNull(calibrator.getEstimatedMzy());
10200 assertNull(calibrator.getEstimatedCovariance());
10201 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10202 assertNotNull(calibrator.getGroundTruthGravityNorm());
10203 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10204 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10205 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10206 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10207 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10208 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10209 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10210
10211
10212 calibrator = null;
10213 try {
10214 calibrator = new KnownGravityNormAccelerometerCalibrator(
10215 -gravityNorm, biasX, biasY, biasZ, this);
10216 fail("IllegalArgumentException expected but not thrown");
10217 } catch (final IllegalArgumentException ignore) {
10218 }
10219 assertNull(calibrator);
10220 }
10221
10222 @Test
10223 public void testConstructor89() throws WrongSizeException {
10224 final Collection<StandardDeviationBodyKinematics> measurements =
10225 Collections.emptyList();
10226
10227 final Matrix ba = generateBa();
10228 final double biasX = ba.getElementAtIndex(0);
10229 final double biasY = ba.getElementAtIndex(1);
10230 final double biasZ = ba.getElementAtIndex(2);
10231
10232 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10233 final double latitude = Math.toRadians(
10234 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10235 final double longitude = Math.toRadians(
10236 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10237 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10238 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10239 final NEDVelocity nedVelocity = new NEDVelocity();
10240 final ECEFPosition ecefPosition = new ECEFPosition();
10241 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10242 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10243 ecefPosition, ecefVelocity);
10244 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10245 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10246 final double gravityNorm = gravity.getNorm();
10247
10248 KnownGravityNormAccelerometerCalibrator calibrator =
10249 new KnownGravityNormAccelerometerCalibrator(
10250 gravityNorm, measurements,
10251 biasX, biasY, biasZ);
10252
10253
10254 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10255 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10256 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10257 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10258 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10259 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10260 final Acceleration bx2 = new Acceleration(0.0,
10261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10262 calibrator.getInitialBiasXAsAcceleration(bx2);
10263 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10264 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10265 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10266 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10267 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10268 final Acceleration by2 = new Acceleration(0.0,
10269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10270 calibrator.getInitialBiasYAsAcceleration(by2);
10271 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10272 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10273 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10274 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10275 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10276 final Acceleration bz2 = new Acceleration(0.0,
10277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10278 calibrator.getInitialBiasZAsAcceleration(bz2);
10279 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10280 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10281 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10282 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10283 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10284 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10285 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10286 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10287 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10288 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10289 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10290 final double[] bias1 = calibrator.getInitialBias();
10291 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10292 final double[] bias2 = new double[3];
10293 calibrator.getInitialBias(bias2);
10294 assertArrayEquals(bias1, bias2, 0.0);
10295 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10296 assertEquals(b1, ba);
10297 final Matrix b2 = new Matrix(3, 1);
10298 calibrator.getInitialBiasAsMatrix(b2);
10299 assertEquals(b1, b2);
10300 final Matrix ma1 = calibrator.getInitialMa();
10301 assertEquals(ma1, new Matrix(3, 3));
10302 final Matrix ma2 = new Matrix(3, 3);
10303 calibrator.getInitialMa(ma2);
10304 assertEquals(ma1, ma2);
10305 assertSame(calibrator.getMeasurements(), measurements);
10306 assertFalse(calibrator.isCommonAxisUsed());
10307 assertNull(calibrator.getListener());
10308 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10309 assertFalse(calibrator.isReady());
10310 assertFalse(calibrator.isRunning());
10311 assertNull(calibrator.getEstimatedBiases());
10312 assertFalse(calibrator.getEstimatedBiases(null));
10313 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10314 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10315 assertNull(calibrator.getEstimatedBiasFx());
10316 assertNull(calibrator.getEstimatedBiasFy());
10317 assertNull(calibrator.getEstimatedBiasFz());
10318 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10319 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10320 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10321 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10322 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10323 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10324 assertNull(calibrator.getEstimatedMa());
10325 assertNull(calibrator.getEstimatedSx());
10326 assertNull(calibrator.getEstimatedSy());
10327 assertNull(calibrator.getEstimatedSz());
10328 assertNull(calibrator.getEstimatedMxy());
10329 assertNull(calibrator.getEstimatedMxz());
10330 assertNull(calibrator.getEstimatedMyx());
10331 assertNull(calibrator.getEstimatedMyz());
10332 assertNull(calibrator.getEstimatedMzx());
10333 assertNull(calibrator.getEstimatedMzy());
10334 assertNull(calibrator.getEstimatedCovariance());
10335 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10336 assertNotNull(calibrator.getGroundTruthGravityNorm());
10337 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10338 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10339 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10340 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10341 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10342 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10343 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10344
10345
10346 calibrator = null;
10347 try {
10348 calibrator = new KnownGravityNormAccelerometerCalibrator(
10349 -gravityNorm, measurements,
10350 biasX, biasY, biasZ);
10351 fail("IllegalArgumentException expected but not thrown");
10352 } catch (final IllegalArgumentException ignore) {
10353 }
10354 assertNull(calibrator);
10355 }
10356
10357 @Test
10358 public void testConstructor90() throws WrongSizeException {
10359 final Collection<StandardDeviationBodyKinematics> measurements =
10360 Collections.emptyList();
10361
10362 final Matrix ba = generateBa();
10363 final double biasX = ba.getElementAtIndex(0);
10364 final double biasY = ba.getElementAtIndex(1);
10365 final double biasZ = ba.getElementAtIndex(2);
10366
10367 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10368 final double latitude = Math.toRadians(
10369 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10370 final double longitude = Math.toRadians(
10371 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10372 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10373 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10374 final NEDVelocity nedVelocity = new NEDVelocity();
10375 final ECEFPosition ecefPosition = new ECEFPosition();
10376 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10377 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10378 ecefPosition, ecefVelocity);
10379 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10380 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10381 final double gravityNorm = gravity.getNorm();
10382
10383 KnownGravityNormAccelerometerCalibrator calibrator =
10384 new KnownGravityNormAccelerometerCalibrator(
10385 gravityNorm, measurements,
10386 biasX, biasY, biasZ, this);
10387
10388
10389 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10390 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10391 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10392 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10393 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10394 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10395 final Acceleration bx2 = new Acceleration(0.0,
10396 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10397 calibrator.getInitialBiasXAsAcceleration(bx2);
10398 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10399 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10400 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10401 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10402 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10403 final Acceleration by2 = new Acceleration(0.0,
10404 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10405 calibrator.getInitialBiasYAsAcceleration(by2);
10406 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10407 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10408 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10409 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10410 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10411 final Acceleration bz2 = new Acceleration(0.0,
10412 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10413 calibrator.getInitialBiasZAsAcceleration(bz2);
10414 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10415 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10416 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10417 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10418 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10419 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10420 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10421 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10422 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10423 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10424 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10425 final double[] bias1 = calibrator.getInitialBias();
10426 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10427 final double[] bias2 = new double[3];
10428 calibrator.getInitialBias(bias2);
10429 assertArrayEquals(bias1, bias2, 0.0);
10430 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10431 assertEquals(b1, ba);
10432 final Matrix b2 = new Matrix(3, 1);
10433 calibrator.getInitialBiasAsMatrix(b2);
10434 assertEquals(b1, b2);
10435 final Matrix ma1 = calibrator.getInitialMa();
10436 assertEquals(ma1, new Matrix(3, 3));
10437 final Matrix ma2 = new Matrix(3, 3);
10438 calibrator.getInitialMa(ma2);
10439 assertEquals(ma1, ma2);
10440 assertSame(calibrator.getMeasurements(), measurements);
10441 assertFalse(calibrator.isCommonAxisUsed());
10442 assertSame(calibrator.getListener(), this);
10443 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10444 assertFalse(calibrator.isReady());
10445 assertFalse(calibrator.isRunning());
10446 assertNull(calibrator.getEstimatedBiases());
10447 assertFalse(calibrator.getEstimatedBiases(null));
10448 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10449 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10450 assertNull(calibrator.getEstimatedBiasFx());
10451 assertNull(calibrator.getEstimatedBiasFy());
10452 assertNull(calibrator.getEstimatedBiasFz());
10453 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10454 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10455 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10456 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10457 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10458 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10459 assertNull(calibrator.getEstimatedMa());
10460 assertNull(calibrator.getEstimatedSx());
10461 assertNull(calibrator.getEstimatedSy());
10462 assertNull(calibrator.getEstimatedSz());
10463 assertNull(calibrator.getEstimatedMxy());
10464 assertNull(calibrator.getEstimatedMxz());
10465 assertNull(calibrator.getEstimatedMyx());
10466 assertNull(calibrator.getEstimatedMyz());
10467 assertNull(calibrator.getEstimatedMzx());
10468 assertNull(calibrator.getEstimatedMzy());
10469 assertNull(calibrator.getEstimatedCovariance());
10470 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10471 assertNotNull(calibrator.getGroundTruthGravityNorm());
10472 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10473 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10474 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10475 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10476 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10477 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10478 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10479
10480
10481 calibrator = null;
10482 try {
10483 calibrator = new KnownGravityNormAccelerometerCalibrator(
10484 -gravityNorm, measurements,
10485 biasX, biasY, biasZ, this);
10486 fail("IllegalArgumentException expected but not thrown");
10487 } catch (final IllegalArgumentException ignore) {
10488 }
10489 assertNull(calibrator);
10490 }
10491
10492 @Test
10493 public void testConstructor91() throws WrongSizeException {
10494 final Matrix ba = generateBa();
10495 final double biasX = ba.getElementAtIndex(0);
10496 final double biasY = ba.getElementAtIndex(1);
10497 final double biasZ = ba.getElementAtIndex(2);
10498
10499 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10500 final double latitude = Math.toRadians(
10501 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10502 final double longitude = Math.toRadians(
10503 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10504 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10505 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10506 final NEDVelocity nedVelocity = new NEDVelocity();
10507 final ECEFPosition ecefPosition = new ECEFPosition();
10508 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10509 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10510 ecefPosition, ecefVelocity);
10511 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10512 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10513 final double gravityNorm = gravity.getNorm();
10514
10515 KnownGravityNormAccelerometerCalibrator calibrator =
10516 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10517 true, biasX, biasY, biasZ);
10518
10519
10520 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10521 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10522 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10523 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10524 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10525 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10526 final Acceleration bx2 = new Acceleration(0.0,
10527 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10528 calibrator.getInitialBiasXAsAcceleration(bx2);
10529 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10530 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10531 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10532 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10533 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10534 final Acceleration by2 = new Acceleration(0.0,
10535 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10536 calibrator.getInitialBiasYAsAcceleration(by2);
10537 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10538 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10539 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10540 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10541 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10542 final Acceleration bz2 = new Acceleration(0.0,
10543 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10544 calibrator.getInitialBiasZAsAcceleration(bz2);
10545 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10546 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10547 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10548 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10549 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10550 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10551 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10552 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10553 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10554 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10555 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10556 final double[] bias1 = calibrator.getInitialBias();
10557 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10558 final double[] bias2 = new double[3];
10559 calibrator.getInitialBias(bias2);
10560 assertArrayEquals(bias1, bias2, 0.0);
10561 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10562 assertEquals(b1, ba);
10563 final Matrix b2 = new Matrix(3, 1);
10564 calibrator.getInitialBiasAsMatrix(b2);
10565 assertEquals(b1, b2);
10566 final Matrix ma1 = calibrator.getInitialMa();
10567 assertEquals(ma1, new Matrix(3, 3));
10568 final Matrix ma2 = new Matrix(3, 3);
10569 calibrator.getInitialMa(ma2);
10570 assertEquals(ma1, ma2);
10571 assertNull(calibrator.getMeasurements());
10572 assertTrue(calibrator.isCommonAxisUsed());
10573 assertNull(calibrator.getListener());
10574 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10575 assertFalse(calibrator.isReady());
10576 assertFalse(calibrator.isRunning());
10577 assertNull(calibrator.getEstimatedBiases());
10578 assertFalse(calibrator.getEstimatedBiases(null));
10579 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10580 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10581 assertNull(calibrator.getEstimatedBiasFx());
10582 assertNull(calibrator.getEstimatedBiasFy());
10583 assertNull(calibrator.getEstimatedBiasFz());
10584 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10585 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10586 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10587 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10588 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10589 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10590 assertNull(calibrator.getEstimatedMa());
10591 assertNull(calibrator.getEstimatedSx());
10592 assertNull(calibrator.getEstimatedSy());
10593 assertNull(calibrator.getEstimatedSz());
10594 assertNull(calibrator.getEstimatedMxy());
10595 assertNull(calibrator.getEstimatedMxz());
10596 assertNull(calibrator.getEstimatedMyx());
10597 assertNull(calibrator.getEstimatedMyz());
10598 assertNull(calibrator.getEstimatedMzx());
10599 assertNull(calibrator.getEstimatedMzy());
10600 assertNull(calibrator.getEstimatedCovariance());
10601 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10602 assertNotNull(calibrator.getGroundTruthGravityNorm());
10603 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10604 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10605 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10606 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10607 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10608 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10609 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10610
10611
10612 calibrator = null;
10613 try {
10614 calibrator = new KnownGravityNormAccelerometerCalibrator(
10615 -gravityNorm, true, biasX, biasY, biasZ);
10616 fail("IllegalArgumentException expected but not thrown");
10617 } catch (final IllegalArgumentException ignore) {
10618 }
10619 assertNull(calibrator);
10620 }
10621
10622 @Test
10623 public void testConstructor92() throws WrongSizeException {
10624 final Matrix ba = generateBa();
10625 final double biasX = ba.getElementAtIndex(0);
10626 final double biasY = ba.getElementAtIndex(1);
10627 final double biasZ = ba.getElementAtIndex(2);
10628
10629 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10630 final double latitude = Math.toRadians(
10631 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10632 final double longitude = Math.toRadians(
10633 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10634 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10635 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10636 final NEDVelocity nedVelocity = new NEDVelocity();
10637 final ECEFPosition ecefPosition = new ECEFPosition();
10638 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10639 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10640 ecefPosition, ecefVelocity);
10641 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10642 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10643 final double gravityNorm = gravity.getNorm();
10644
10645 KnownGravityNormAccelerometerCalibrator calibrator =
10646 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10647 true, biasX, biasY, biasZ,
10648 this);
10649
10650
10651 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10652 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10653 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10654 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10655 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10656 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10657 final Acceleration bx2 = new Acceleration(0.0,
10658 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10659 calibrator.getInitialBiasXAsAcceleration(bx2);
10660 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10661 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10662 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10663 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10664 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10665 final Acceleration by2 = new Acceleration(0.0,
10666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10667 calibrator.getInitialBiasYAsAcceleration(by2);
10668 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10669 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10670 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10671 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10672 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10673 final Acceleration bz2 = new Acceleration(0.0,
10674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10675 calibrator.getInitialBiasZAsAcceleration(bz2);
10676 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10677 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10678 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10679 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10680 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10681 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10682 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10683 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10684 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10685 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10686 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10687 final double[] bias1 = calibrator.getInitialBias();
10688 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10689 final double[] bias2 = new double[3];
10690 calibrator.getInitialBias(bias2);
10691 assertArrayEquals(bias1, bias2, 0.0);
10692 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10693 assertEquals(b1, ba);
10694 final Matrix b2 = new Matrix(3, 1);
10695 calibrator.getInitialBiasAsMatrix(b2);
10696 assertEquals(b1, b2);
10697 final Matrix ma1 = calibrator.getInitialMa();
10698 assertEquals(ma1, new Matrix(3, 3));
10699 final Matrix ma2 = new Matrix(3, 3);
10700 calibrator.getInitialMa(ma2);
10701 assertEquals(ma1, ma2);
10702 assertNull(calibrator.getMeasurements());
10703 assertTrue(calibrator.isCommonAxisUsed());
10704 assertSame(calibrator.getListener(), this);
10705 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10706 assertFalse(calibrator.isReady());
10707 assertFalse(calibrator.isRunning());
10708 assertNull(calibrator.getEstimatedBiases());
10709 assertFalse(calibrator.getEstimatedBiases(null));
10710 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10711 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10712 assertNull(calibrator.getEstimatedBiasFx());
10713 assertNull(calibrator.getEstimatedBiasFy());
10714 assertNull(calibrator.getEstimatedBiasFz());
10715 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10716 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10717 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10718 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10719 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10720 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10721 assertNull(calibrator.getEstimatedMa());
10722 assertNull(calibrator.getEstimatedSx());
10723 assertNull(calibrator.getEstimatedSy());
10724 assertNull(calibrator.getEstimatedSz());
10725 assertNull(calibrator.getEstimatedMxy());
10726 assertNull(calibrator.getEstimatedMxz());
10727 assertNull(calibrator.getEstimatedMyx());
10728 assertNull(calibrator.getEstimatedMyz());
10729 assertNull(calibrator.getEstimatedMzx());
10730 assertNull(calibrator.getEstimatedMzy());
10731 assertNull(calibrator.getEstimatedCovariance());
10732 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10733 assertNotNull(calibrator.getGroundTruthGravityNorm());
10734 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10735 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10736 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10737 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10738 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10739 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10740 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10741
10742
10743 calibrator = null;
10744 try {
10745 calibrator = new KnownGravityNormAccelerometerCalibrator(
10746 -gravityNorm, true, biasX, biasY, biasZ,
10747 this);
10748 fail("IllegalArgumentException expected but not thrown");
10749 } catch (final IllegalArgumentException ignore) {
10750 }
10751 assertNull(calibrator);
10752 }
10753
10754 @Test
10755 public void testConstructor93() throws WrongSizeException {
10756 final Collection<StandardDeviationBodyKinematics> measurements =
10757 Collections.emptyList();
10758
10759 final Matrix ba = generateBa();
10760 final double biasX = ba.getElementAtIndex(0);
10761 final double biasY = ba.getElementAtIndex(1);
10762 final double biasZ = ba.getElementAtIndex(2);
10763
10764 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10765 final double latitude = Math.toRadians(
10766 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10767 final double longitude = Math.toRadians(
10768 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10769 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10770 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10771 final NEDVelocity nedVelocity = new NEDVelocity();
10772 final ECEFPosition ecefPosition = new ECEFPosition();
10773 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10774 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10775 ecefPosition, ecefVelocity);
10776 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10777 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10778 final double gravityNorm = gravity.getNorm();
10779
10780 KnownGravityNormAccelerometerCalibrator calibrator =
10781 new KnownGravityNormAccelerometerCalibrator(
10782 gravityNorm, measurements,
10783 true, biasX, biasY, biasZ);
10784
10785
10786 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10787 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10788 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10789 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10790 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10791 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10792 final Acceleration bx2 = new Acceleration(0.0,
10793 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10794 calibrator.getInitialBiasXAsAcceleration(bx2);
10795 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10796 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10797 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10798 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10799 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10800 final Acceleration by2 = new Acceleration(0.0,
10801 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10802 calibrator.getInitialBiasYAsAcceleration(by2);
10803 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10804 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10805 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10806 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10807 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10808 final Acceleration bz2 = new Acceleration(0.0,
10809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10810 calibrator.getInitialBiasZAsAcceleration(bz2);
10811 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10812 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10813 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10814 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10815 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10816 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10817 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10818 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10819 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10820 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10821 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10822 final double[] bias1 = calibrator.getInitialBias();
10823 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10824 final double[] bias2 = new double[3];
10825 calibrator.getInitialBias(bias2);
10826 assertArrayEquals(bias1, bias2, 0.0);
10827 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10828 assertEquals(b1, ba);
10829 final Matrix b2 = new Matrix(3, 1);
10830 calibrator.getInitialBiasAsMatrix(b2);
10831 assertEquals(b1, b2);
10832 final Matrix ma1 = calibrator.getInitialMa();
10833 assertEquals(ma1, new Matrix(3, 3));
10834 final Matrix ma2 = new Matrix(3, 3);
10835 calibrator.getInitialMa(ma2);
10836 assertEquals(ma1, ma2);
10837 assertSame(calibrator.getMeasurements(), measurements);
10838 assertTrue(calibrator.isCommonAxisUsed());
10839 assertNull(calibrator.getListener());
10840 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10841 assertFalse(calibrator.isReady());
10842 assertFalse(calibrator.isRunning());
10843 assertNull(calibrator.getEstimatedBiases());
10844 assertFalse(calibrator.getEstimatedBiases(null));
10845 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10846 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10847 assertNull(calibrator.getEstimatedBiasFx());
10848 assertNull(calibrator.getEstimatedBiasFy());
10849 assertNull(calibrator.getEstimatedBiasFz());
10850 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10851 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10852 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10853 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10854 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10855 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10856 assertNull(calibrator.getEstimatedMa());
10857 assertNull(calibrator.getEstimatedSx());
10858 assertNull(calibrator.getEstimatedSy());
10859 assertNull(calibrator.getEstimatedSz());
10860 assertNull(calibrator.getEstimatedMxy());
10861 assertNull(calibrator.getEstimatedMxz());
10862 assertNull(calibrator.getEstimatedMyx());
10863 assertNull(calibrator.getEstimatedMyz());
10864 assertNull(calibrator.getEstimatedMzx());
10865 assertNull(calibrator.getEstimatedMzy());
10866 assertNull(calibrator.getEstimatedCovariance());
10867 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10868 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10869 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10870 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10871 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10872 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10873 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10874 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10875
10876
10877 calibrator = null;
10878 try {
10879 calibrator = new KnownGravityNormAccelerometerCalibrator(
10880 -gravityNorm, measurements,
10881 true, biasX, biasY, biasZ);
10882 fail("IllegalArgumentException expected but not thrown");
10883 } catch (final IllegalArgumentException ignore) {
10884 }
10885 assertNull(calibrator);
10886 }
10887
10888 @Test
10889 public void testConstructor94() throws WrongSizeException {
10890 final Collection<StandardDeviationBodyKinematics> measurements =
10891 Collections.emptyList();
10892
10893 final Matrix ba = generateBa();
10894 final double biasX = ba.getElementAtIndex(0);
10895 final double biasY = ba.getElementAtIndex(1);
10896 final double biasZ = ba.getElementAtIndex(2);
10897
10898 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10899 final double latitude = Math.toRadians(
10900 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10901 final double longitude = Math.toRadians(
10902 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10903 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10904 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10905 final NEDVelocity nedVelocity = new NEDVelocity();
10906 final ECEFPosition ecefPosition = new ECEFPosition();
10907 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10908 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10909 ecefPosition, ecefVelocity);
10910 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10911 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10912 final double gravityNorm = gravity.getNorm();
10913
10914 KnownGravityNormAccelerometerCalibrator calibrator =
10915 new KnownGravityNormAccelerometerCalibrator(
10916 gravityNorm, measurements,
10917 true, biasX, biasY, biasZ, this);
10918
10919
10920 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10921 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10922 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10923 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10924 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10926 final Acceleration bx2 = new Acceleration(0.0,
10927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10928 calibrator.getInitialBiasXAsAcceleration(bx2);
10929 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10930 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10931 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10932 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10933 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10934 final Acceleration by2 = new Acceleration(0.0,
10935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10936 calibrator.getInitialBiasYAsAcceleration(by2);
10937 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10938 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10939 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10940 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10941 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10942 final Acceleration bz2 = new Acceleration(0.0,
10943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10944 calibrator.getInitialBiasZAsAcceleration(bz2);
10945 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10946 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10947 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10948 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10949 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10950 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10951 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10952 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10953 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10954 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10955 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10956 final double[] bias1 = calibrator.getInitialBias();
10957 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10958 final double[] bias2 = new double[3];
10959 calibrator.getInitialBias(bias2);
10960 assertArrayEquals(bias1, bias2, 0.0);
10961 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10962 assertEquals(b1, ba);
10963 final Matrix b2 = new Matrix(3, 1);
10964 calibrator.getInitialBiasAsMatrix(b2);
10965 assertEquals(b1, b2);
10966 final Matrix ma1 = calibrator.getInitialMa();
10967 assertEquals(ma1, new Matrix(3, 3));
10968 final Matrix ma2 = new Matrix(3, 3);
10969 calibrator.getInitialMa(ma2);
10970 assertEquals(ma1, ma2);
10971 assertSame(calibrator.getMeasurements(), measurements);
10972 assertTrue(calibrator.isCommonAxisUsed());
10973 assertSame(calibrator.getListener(), this);
10974 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10975 assertFalse(calibrator.isReady());
10976 assertFalse(calibrator.isRunning());
10977 assertNull(calibrator.getEstimatedBiases());
10978 assertFalse(calibrator.getEstimatedBiases(null));
10979 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10980 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10981 assertNull(calibrator.getEstimatedBiasFx());
10982 assertNull(calibrator.getEstimatedBiasFy());
10983 assertNull(calibrator.getEstimatedBiasFz());
10984 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10985 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10986 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10987 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10988 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10989 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10990 assertNull(calibrator.getEstimatedMa());
10991 assertNull(calibrator.getEstimatedSx());
10992 assertNull(calibrator.getEstimatedSy());
10993 assertNull(calibrator.getEstimatedSz());
10994 assertNull(calibrator.getEstimatedMxy());
10995 assertNull(calibrator.getEstimatedMxz());
10996 assertNull(calibrator.getEstimatedMyx());
10997 assertNull(calibrator.getEstimatedMyz());
10998 assertNull(calibrator.getEstimatedMzx());
10999 assertNull(calibrator.getEstimatedMzy());
11000 assertNull(calibrator.getEstimatedCovariance());
11001 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11002 assertNotNull(calibrator.getGroundTruthGravityNorm());
11003 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11004 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11005 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11006 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11007 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11008 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11009 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11010
11011
11012 calibrator = null;
11013 try {
11014 calibrator = new KnownGravityNormAccelerometerCalibrator(
11015 -gravityNorm, measurements,
11016 true, biasX, biasY, biasZ, this);
11017 fail("IllegalArgumentException expected but not thrown");
11018 } catch (final IllegalArgumentException ignore) {
11019 }
11020 assertNull(calibrator);
11021 }
11022
11023 @Test
11024 public void testConstructor95() throws WrongSizeException {
11025 final Matrix ba = generateBa();
11026 final double biasX = ba.getElementAtIndex(0);
11027 final double biasY = ba.getElementAtIndex(1);
11028 final double biasZ = ba.getElementAtIndex(2);
11029
11030 final Acceleration bx = new Acceleration(biasX,
11031 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11032 final Acceleration by = new Acceleration(biasY,
11033 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11034 final Acceleration bz = new Acceleration(biasZ,
11035 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11036
11037 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11038 final double latitude = Math.toRadians(
11039 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11040 final double longitude = Math.toRadians(
11041 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11042 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11043 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11044 final NEDVelocity nedVelocity = new NEDVelocity();
11045 final ECEFPosition ecefPosition = new ECEFPosition();
11046 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11047 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11048 ecefPosition, ecefVelocity);
11049 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11050 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11051 final double gravityNorm = gravity.getNorm();
11052
11053 KnownGravityNormAccelerometerCalibrator calibrator =
11054 new KnownGravityNormAccelerometerCalibrator(
11055 gravityNorm, bx, by, bz);
11056
11057
11058 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11059 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11060 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11061 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11062 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11063 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11064 final Acceleration bx2 = new Acceleration(0.0,
11065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11066 calibrator.getInitialBiasXAsAcceleration(bx2);
11067 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11068 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11069 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11070 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11071 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11072 final Acceleration by2 = new Acceleration(0.0,
11073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11074 calibrator.getInitialBiasYAsAcceleration(by2);
11075 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11076 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11077 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11078 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11079 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11080 final Acceleration bz2 = new Acceleration(0.0,
11081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11082 calibrator.getInitialBiasZAsAcceleration(bz2);
11083 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11084 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11085 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11086 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11087 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11088 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11089 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11090 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11091 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11092 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11093 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11094 final double[] bias1 = calibrator.getInitialBias();
11095 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11096 final double[] bias2 = new double[3];
11097 calibrator.getInitialBias(bias2);
11098 assertArrayEquals(bias1, bias2, 0.0);
11099 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11100 assertEquals(b1, ba);
11101 final Matrix b2 = new Matrix(3, 1);
11102 calibrator.getInitialBiasAsMatrix(b2);
11103 assertEquals(b1, b2);
11104 final Matrix ma1 = calibrator.getInitialMa();
11105 assertEquals(ma1, new Matrix(3, 3));
11106 final Matrix ma2 = new Matrix(3, 3);
11107 calibrator.getInitialMa(ma2);
11108 assertEquals(ma1, ma2);
11109 assertNull(calibrator.getMeasurements());
11110 assertFalse(calibrator.isCommonAxisUsed());
11111 assertNull(calibrator.getListener());
11112 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11113 assertFalse(calibrator.isReady());
11114 assertFalse(calibrator.isRunning());
11115 assertNull(calibrator.getEstimatedBiases());
11116 assertFalse(calibrator.getEstimatedBiases(null));
11117 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11118 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11119 assertNull(calibrator.getEstimatedBiasFx());
11120 assertNull(calibrator.getEstimatedBiasFy());
11121 assertNull(calibrator.getEstimatedBiasFz());
11122 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11123 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11124 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11125 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11126 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11127 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11128 assertNull(calibrator.getEstimatedMa());
11129 assertNull(calibrator.getEstimatedSx());
11130 assertNull(calibrator.getEstimatedSy());
11131 assertNull(calibrator.getEstimatedSz());
11132 assertNull(calibrator.getEstimatedMxy());
11133 assertNull(calibrator.getEstimatedMxz());
11134 assertNull(calibrator.getEstimatedMyx());
11135 assertNull(calibrator.getEstimatedMyz());
11136 assertNull(calibrator.getEstimatedMzx());
11137 assertNull(calibrator.getEstimatedMzy());
11138 assertNull(calibrator.getEstimatedCovariance());
11139 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11140 assertNotNull(calibrator.getGroundTruthGravityNorm());
11141 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11142 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11143 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11144 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11145 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11146 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11147 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11148
11149
11150 calibrator = null;
11151 try {
11152 calibrator = new KnownGravityNormAccelerometerCalibrator(
11153 -gravityNorm, bx, by, bz);
11154 fail("IllegalArgumentException expected but not thrown");
11155 } catch (final IllegalArgumentException ignore) {
11156 }
11157 assertNull(calibrator);
11158 }
11159
11160 @Test
11161 public void testConstructor96() throws WrongSizeException {
11162 final Matrix ba = generateBa();
11163 final double biasX = ba.getElementAtIndex(0);
11164 final double biasY = ba.getElementAtIndex(1);
11165 final double biasZ = ba.getElementAtIndex(2);
11166
11167 final Acceleration bx = new Acceleration(biasX,
11168 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11169 final Acceleration by = new Acceleration(biasY,
11170 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11171 final Acceleration bz = new Acceleration(biasZ,
11172 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11173
11174 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11175 final double latitude = Math.toRadians(
11176 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11177 final double longitude = Math.toRadians(
11178 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11179 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11180 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11181 final NEDVelocity nedVelocity = new NEDVelocity();
11182 final ECEFPosition ecefPosition = new ECEFPosition();
11183 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11184 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11185 ecefPosition, ecefVelocity);
11186 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11187 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11188 final double gravityNorm = gravity.getNorm();
11189
11190 KnownGravityNormAccelerometerCalibrator calibrator =
11191 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11192 bx, by, bz, this);
11193
11194
11195 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11196 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11197 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11198 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11199 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11200 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11201 final Acceleration bx2 = new Acceleration(0.0,
11202 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11203 calibrator.getInitialBiasXAsAcceleration(bx2);
11204 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11205 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11206 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11207 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11208 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11209 final Acceleration by2 = new Acceleration(0.0,
11210 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11211 calibrator.getInitialBiasYAsAcceleration(by2);
11212 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11213 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11214 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11215 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11216 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11217 final Acceleration bz2 = new Acceleration(0.0,
11218 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11219 calibrator.getInitialBiasZAsAcceleration(bz2);
11220 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11221 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11222 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11223 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11224 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11225 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11226 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11227 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11228 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11229 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11230 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11231 final double[] bias1 = calibrator.getInitialBias();
11232 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11233 final double[] bias2 = new double[3];
11234 calibrator.getInitialBias(bias2);
11235 assertArrayEquals(bias1, bias2, 0.0);
11236 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11237 assertEquals(b1, ba);
11238 final Matrix b2 = new Matrix(3, 1);
11239 calibrator.getInitialBiasAsMatrix(b2);
11240 assertEquals(b1, b2);
11241 final Matrix ma1 = calibrator.getInitialMa();
11242 assertEquals(ma1, new Matrix(3, 3));
11243 final Matrix ma2 = new Matrix(3, 3);
11244 calibrator.getInitialMa(ma2);
11245 assertEquals(ma1, ma2);
11246 assertNull(calibrator.getMeasurements());
11247 assertFalse(calibrator.isCommonAxisUsed());
11248 assertSame(calibrator.getListener(), this);
11249 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11250 assertFalse(calibrator.isReady());
11251 assertFalse(calibrator.isRunning());
11252 assertNull(calibrator.getEstimatedBiases());
11253 assertFalse(calibrator.getEstimatedBiases(null));
11254 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11255 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11256 assertNull(calibrator.getEstimatedBiasFx());
11257 assertNull(calibrator.getEstimatedBiasFy());
11258 assertNull(calibrator.getEstimatedBiasFz());
11259 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11260 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11261 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11262 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11263 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11264 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11265 assertNull(calibrator.getEstimatedMa());
11266 assertNull(calibrator.getEstimatedSx());
11267 assertNull(calibrator.getEstimatedSy());
11268 assertNull(calibrator.getEstimatedSz());
11269 assertNull(calibrator.getEstimatedMxy());
11270 assertNull(calibrator.getEstimatedMxz());
11271 assertNull(calibrator.getEstimatedMyx());
11272 assertNull(calibrator.getEstimatedMyz());
11273 assertNull(calibrator.getEstimatedMzx());
11274 assertNull(calibrator.getEstimatedMzy());
11275 assertNull(calibrator.getEstimatedCovariance());
11276 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11277 assertNotNull(calibrator.getGroundTruthGravityNorm());
11278 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11279 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11280 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11281 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11282 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11283 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11284 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11285
11286
11287 calibrator = null;
11288 try {
11289 calibrator =
11290 new KnownGravityNormAccelerometerCalibrator(
11291 -gravityNorm, bx, by, bz, this);
11292 fail("IllegalArgumentException but not thrown");
11293 } catch (final IllegalArgumentException ignore) {
11294 }
11295 assertNull(calibrator);
11296 }
11297
11298 @Test
11299 public void testConstructor97() throws WrongSizeException {
11300 final Collection<StandardDeviationBodyKinematics> measurements =
11301 Collections.emptyList();
11302
11303 final Matrix ba = generateBa();
11304 final double biasX = ba.getElementAtIndex(0);
11305 final double biasY = ba.getElementAtIndex(1);
11306 final double biasZ = ba.getElementAtIndex(2);
11307
11308 final Acceleration bx = new Acceleration(biasX,
11309 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11310 final Acceleration by = new Acceleration(biasY,
11311 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11312 final Acceleration bz = new Acceleration(biasZ,
11313 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11314
11315 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11316 final double latitude = Math.toRadians(
11317 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11318 final double longitude = Math.toRadians(
11319 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11320 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11321 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11322 final NEDVelocity nedVelocity = new NEDVelocity();
11323 final ECEFPosition ecefPosition = new ECEFPosition();
11324 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11325 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11326 ecefPosition, ecefVelocity);
11327 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11328 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11329 final double gravityNorm = gravity.getNorm();
11330
11331 KnownGravityNormAccelerometerCalibrator calibrator =
11332 new KnownGravityNormAccelerometerCalibrator(
11333 gravityNorm, measurements, bx, by, bz);
11334
11335
11336 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11337 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11338 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11339 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11340 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11341 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11342 final Acceleration bx2 = new Acceleration(0.0,
11343 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11344 calibrator.getInitialBiasXAsAcceleration(bx2);
11345 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11346 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11347 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11348 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11349 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11350 final Acceleration by2 = new Acceleration(0.0,
11351 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11352 calibrator.getInitialBiasYAsAcceleration(by2);
11353 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11354 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11355 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11356 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11357 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11358 final Acceleration bz2 = new Acceleration(0.0,
11359 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11360 calibrator.getInitialBiasZAsAcceleration(bz2);
11361 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11362 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11363 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11364 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11365 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11366 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11367 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11368 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11369 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11370 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11371 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11372 final double[] bias1 = calibrator.getInitialBias();
11373 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11374 final double[] bias2 = new double[3];
11375 calibrator.getInitialBias(bias2);
11376 assertArrayEquals(bias1, bias2, 0.0);
11377 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11378 assertEquals(b1, ba);
11379 final Matrix b2 = new Matrix(3, 1);
11380 calibrator.getInitialBiasAsMatrix(b2);
11381 assertEquals(b1, b2);
11382 final Matrix ma1 = calibrator.getInitialMa();
11383 assertEquals(ma1, new Matrix(3, 3));
11384 final Matrix ma2 = new Matrix(3, 3);
11385 calibrator.getInitialMa(ma2);
11386 assertEquals(ma1, ma2);
11387 assertSame(calibrator.getMeasurements(), measurements);
11388 assertFalse(calibrator.isCommonAxisUsed());
11389 assertNull(calibrator.getListener());
11390 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11391 assertFalse(calibrator.isReady());
11392 assertFalse(calibrator.isRunning());
11393 assertNull(calibrator.getEstimatedBiases());
11394 assertFalse(calibrator.getEstimatedBiases(null));
11395 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11396 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11397 assertNull(calibrator.getEstimatedBiasFx());
11398 assertNull(calibrator.getEstimatedBiasFy());
11399 assertNull(calibrator.getEstimatedBiasFz());
11400 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11401 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11402 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11403 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11404 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11405 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11406 assertNull(calibrator.getEstimatedMa());
11407 assertNull(calibrator.getEstimatedSx());
11408 assertNull(calibrator.getEstimatedSy());
11409 assertNull(calibrator.getEstimatedSz());
11410 assertNull(calibrator.getEstimatedMxy());
11411 assertNull(calibrator.getEstimatedMxz());
11412 assertNull(calibrator.getEstimatedMyx());
11413 assertNull(calibrator.getEstimatedMyz());
11414 assertNull(calibrator.getEstimatedMzx());
11415 assertNull(calibrator.getEstimatedMzy());
11416 assertNull(calibrator.getEstimatedCovariance());
11417 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11418 assertNotNull(calibrator.getGroundTruthGravityNorm());
11419 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11420 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11421 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11422 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11423 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11424 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11425 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11426
11427
11428 calibrator = null;
11429 try {
11430 calibrator = new KnownGravityNormAccelerometerCalibrator(
11431 -gravityNorm, measurements, bx, by, bz);
11432 fail("IllegalArgumentException expected but not thrown");
11433 } catch (final IllegalArgumentException ignore) {
11434 }
11435 assertNull(calibrator);
11436 }
11437
11438 @Test
11439 public void testConstructor98() throws WrongSizeException {
11440 final Collection<StandardDeviationBodyKinematics> measurements =
11441 Collections.emptyList();
11442
11443 final Matrix ba = generateBa();
11444 final double biasX = ba.getElementAtIndex(0);
11445 final double biasY = ba.getElementAtIndex(1);
11446 final double biasZ = ba.getElementAtIndex(2);
11447
11448 final Acceleration bx = new Acceleration(biasX,
11449 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11450 final Acceleration by = new Acceleration(biasY,
11451 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11452 final Acceleration bz = new Acceleration(biasZ,
11453 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11454
11455 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11456 final double latitude = Math.toRadians(
11457 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11458 final double longitude = Math.toRadians(
11459 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11460 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11461 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11462 final NEDVelocity nedVelocity = new NEDVelocity();
11463 final ECEFPosition ecefPosition = new ECEFPosition();
11464 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11465 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11466 ecefPosition, ecefVelocity);
11467 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11468 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11469 final double gravityNorm = gravity.getNorm();
11470
11471 KnownGravityNormAccelerometerCalibrator calibrator =
11472 new KnownGravityNormAccelerometerCalibrator(
11473 gravityNorm, measurements,
11474 bx, by, bz, this);
11475
11476
11477 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11478 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11479 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11480 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11481 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11482 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11483 final Acceleration bx2 = new Acceleration(0.0,
11484 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11485 calibrator.getInitialBiasXAsAcceleration(bx2);
11486 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11487 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11488 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11489 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11490 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11491 final Acceleration by2 = new Acceleration(0.0,
11492 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11493 calibrator.getInitialBiasYAsAcceleration(by2);
11494 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11495 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11496 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11497 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11498 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11499 final Acceleration bz2 = new Acceleration(0.0,
11500 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11501 calibrator.getInitialBiasZAsAcceleration(bz2);
11502 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11503 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11504 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11505 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11506 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11507 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11508 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11509 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11510 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11511 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11512 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11513 final double[] bias1 = calibrator.getInitialBias();
11514 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11515 final double[] bias2 = new double[3];
11516 calibrator.getInitialBias(bias2);
11517 assertArrayEquals(bias1, bias2, 0.0);
11518 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11519 assertEquals(b1, ba);
11520 final Matrix b2 = new Matrix(3, 1);
11521 calibrator.getInitialBiasAsMatrix(b2);
11522 assertEquals(b1, b2);
11523 final Matrix ma1 = calibrator.getInitialMa();
11524 assertEquals(ma1, new Matrix(3, 3));
11525 final Matrix ma2 = new Matrix(3, 3);
11526 calibrator.getInitialMa(ma2);
11527 assertEquals(ma1, ma2);
11528 assertSame(calibrator.getMeasurements(), measurements);
11529 assertFalse(calibrator.isCommonAxisUsed());
11530 assertSame(calibrator.getListener(), this);
11531 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11532 assertFalse(calibrator.isReady());
11533 assertFalse(calibrator.isRunning());
11534 assertNull(calibrator.getEstimatedBiases());
11535 assertFalse(calibrator.getEstimatedBiases(null));
11536 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11537 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11538 assertNull(calibrator.getEstimatedBiasFx());
11539 assertNull(calibrator.getEstimatedBiasFy());
11540 assertNull(calibrator.getEstimatedBiasFz());
11541 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11542 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11543 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11544 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11545 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11546 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11547 assertNull(calibrator.getEstimatedMa());
11548 assertNull(calibrator.getEstimatedSx());
11549 assertNull(calibrator.getEstimatedSy());
11550 assertNull(calibrator.getEstimatedSz());
11551 assertNull(calibrator.getEstimatedMxy());
11552 assertNull(calibrator.getEstimatedMxz());
11553 assertNull(calibrator.getEstimatedMyx());
11554 assertNull(calibrator.getEstimatedMyz());
11555 assertNull(calibrator.getEstimatedMzx());
11556 assertNull(calibrator.getEstimatedMzy());
11557 assertNull(calibrator.getEstimatedCovariance());
11558 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11559 assertNotNull(calibrator.getGroundTruthGravityNorm());
11560 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11561 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11562 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11563 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11564 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11565 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11566 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11567
11568
11569 calibrator = null;
11570 try {
11571 calibrator = new KnownGravityNormAccelerometerCalibrator(
11572 -gravityNorm, measurements,
11573 bx, by, bz, this);
11574 fail("IllegalArgumentException expected but not thrown");
11575 } catch (final IllegalArgumentException ignore) {
11576 }
11577 assertNull(calibrator);
11578 }
11579
11580 @Test
11581 public void testConstructor99() throws WrongSizeException {
11582 final Matrix ba = generateBa();
11583 final double biasX = ba.getElementAtIndex(0);
11584 final double biasY = ba.getElementAtIndex(1);
11585 final double biasZ = ba.getElementAtIndex(2);
11586
11587 final Acceleration bx = new Acceleration(biasX,
11588 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11589 final Acceleration by = new Acceleration(biasY,
11590 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11591 final Acceleration bz = new Acceleration(biasZ,
11592 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11593
11594 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11595 final double latitude = Math.toRadians(
11596 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11597 final double longitude = Math.toRadians(
11598 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11599 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11600 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11601 final NEDVelocity nedVelocity = new NEDVelocity();
11602 final ECEFPosition ecefPosition = new ECEFPosition();
11603 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11604 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11605 ecefPosition, ecefVelocity);
11606 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11607 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11608 final double gravityNorm = gravity.getNorm();
11609
11610 KnownGravityNormAccelerometerCalibrator calibrator =
11611 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11612 true, bx, by, bz);
11613
11614
11615 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11616 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11617 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11618 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11619 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11620 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11621 final Acceleration bx2 = new Acceleration(0.0,
11622 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11623 calibrator.getInitialBiasXAsAcceleration(bx2);
11624 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11625 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11626 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11627 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11628 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11629 final Acceleration by2 = new Acceleration(0.0,
11630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11631 calibrator.getInitialBiasYAsAcceleration(by2);
11632 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11633 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11634 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11635 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11636 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11637 final Acceleration bz2 = new Acceleration(0.0,
11638 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11639 calibrator.getInitialBiasZAsAcceleration(bz2);
11640 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11641 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11642 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11643 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11644 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11645 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11646 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11647 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11648 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11649 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11650 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11651 final double[] bias1 = calibrator.getInitialBias();
11652 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11653 final double[] bias2 = new double[3];
11654 calibrator.getInitialBias(bias2);
11655 assertArrayEquals(bias1, bias2, 0.0);
11656 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11657 assertEquals(b1, ba);
11658 final Matrix b2 = new Matrix(3, 1);
11659 calibrator.getInitialBiasAsMatrix(b2);
11660 assertEquals(b1, b2);
11661 final Matrix ma1 = calibrator.getInitialMa();
11662 assertEquals(ma1, new Matrix(3, 3));
11663 final Matrix ma2 = new Matrix(3, 3);
11664 calibrator.getInitialMa(ma2);
11665 assertEquals(ma1, ma2);
11666 assertNull(calibrator.getMeasurements());
11667 assertTrue(calibrator.isCommonAxisUsed());
11668 assertNull(calibrator.getListener());
11669 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11670 assertFalse(calibrator.isReady());
11671 assertFalse(calibrator.isRunning());
11672 assertNull(calibrator.getEstimatedBiases());
11673 assertFalse(calibrator.getEstimatedBiases(null));
11674 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11675 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11676 assertNull(calibrator.getEstimatedBiasFx());
11677 assertNull(calibrator.getEstimatedBiasFy());
11678 assertNull(calibrator.getEstimatedBiasFz());
11679 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11680 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11681 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11682 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11683 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11684 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11685 assertNull(calibrator.getEstimatedMa());
11686 assertNull(calibrator.getEstimatedSx());
11687 assertNull(calibrator.getEstimatedSy());
11688 assertNull(calibrator.getEstimatedSz());
11689 assertNull(calibrator.getEstimatedMxy());
11690 assertNull(calibrator.getEstimatedMxz());
11691 assertNull(calibrator.getEstimatedMyx());
11692 assertNull(calibrator.getEstimatedMyz());
11693 assertNull(calibrator.getEstimatedMzx());
11694 assertNull(calibrator.getEstimatedMzy());
11695 assertNull(calibrator.getEstimatedCovariance());
11696 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11697 assertNotNull(calibrator.getGroundTruthGravityNorm());
11698 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11699 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11700 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11701 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11702 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11703 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11704 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11705
11706
11707 calibrator = null;
11708 try {
11709 calibrator = new KnownGravityNormAccelerometerCalibrator(
11710 -gravityNorm, true, bx, by, bz);
11711 fail("IllegalArgumentException expected but not thrown");
11712 } catch (final IllegalArgumentException ignore) {
11713 }
11714 assertNull(calibrator);
11715 }
11716
11717 @Test
11718 public void testConstructor100() throws WrongSizeException {
11719 final Matrix ba = generateBa();
11720 final double biasX = ba.getElementAtIndex(0);
11721 final double biasY = ba.getElementAtIndex(1);
11722 final double biasZ = ba.getElementAtIndex(2);
11723
11724 final Acceleration bx = new Acceleration(biasX,
11725 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11726 final Acceleration by = new Acceleration(biasY,
11727 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11728 final Acceleration bz = new Acceleration(biasZ,
11729 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11730
11731 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11732 final double latitude = Math.toRadians(
11733 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11734 final double longitude = Math.toRadians(
11735 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11736 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11737 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11738 final NEDVelocity nedVelocity = new NEDVelocity();
11739 final ECEFPosition ecefPosition = new ECEFPosition();
11740 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11741 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11742 ecefPosition, ecefVelocity);
11743 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11744 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11745 final double gravityNorm = gravity.getNorm();
11746
11747 KnownGravityNormAccelerometerCalibrator calibrator =
11748 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11749 true, bx, by, bz, this);
11750
11751
11752 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11753 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11754 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11755 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11756 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11757 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11758 final Acceleration bx2 = new Acceleration(0.0,
11759 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11760 calibrator.getInitialBiasXAsAcceleration(bx2);
11761 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11762 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11763 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11764 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11765 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11766 final Acceleration by2 = new Acceleration(0.0,
11767 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11768 calibrator.getInitialBiasYAsAcceleration(by2);
11769 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11770 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11771 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11772 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11773 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11774 final Acceleration bz2 = new Acceleration(0.0,
11775 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11776 calibrator.getInitialBiasZAsAcceleration(bz2);
11777 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11778 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11779 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11780 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11781 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11782 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11783 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11784 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11785 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11786 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11787 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11788 final double[] bias1 = calibrator.getInitialBias();
11789 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11790 final double[] bias2 = new double[3];
11791 calibrator.getInitialBias(bias2);
11792 assertArrayEquals(bias1, bias2, 0.0);
11793 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11794 assertEquals(b1, ba);
11795 final Matrix b2 = new Matrix(3, 1);
11796 calibrator.getInitialBiasAsMatrix(b2);
11797 assertEquals(b1, b2);
11798 final Matrix ma1 = calibrator.getInitialMa();
11799 assertEquals(ma1, new Matrix(3, 3));
11800 final Matrix ma2 = new Matrix(3, 3);
11801 calibrator.getInitialMa(ma2);
11802 assertEquals(ma1, ma2);
11803 assertNull(calibrator.getMeasurements());
11804 assertTrue(calibrator.isCommonAxisUsed());
11805 assertSame(calibrator.getListener(), this);
11806 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11807 assertFalse(calibrator.isReady());
11808 assertFalse(calibrator.isRunning());
11809 assertNull(calibrator.getEstimatedBiases());
11810 assertFalse(calibrator.getEstimatedBiases(null));
11811 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11812 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11813 assertNull(calibrator.getEstimatedBiasFx());
11814 assertNull(calibrator.getEstimatedBiasFy());
11815 assertNull(calibrator.getEstimatedBiasFz());
11816 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11817 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11818 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11819 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11820 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11821 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11822 assertNull(calibrator.getEstimatedMa());
11823 assertNull(calibrator.getEstimatedSx());
11824 assertNull(calibrator.getEstimatedSy());
11825 assertNull(calibrator.getEstimatedSz());
11826 assertNull(calibrator.getEstimatedMxy());
11827 assertNull(calibrator.getEstimatedMxz());
11828 assertNull(calibrator.getEstimatedMyx());
11829 assertNull(calibrator.getEstimatedMyz());
11830 assertNull(calibrator.getEstimatedMzx());
11831 assertNull(calibrator.getEstimatedMzy());
11832 assertNull(calibrator.getEstimatedCovariance());
11833 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11834 assertNotNull(calibrator.getGroundTruthGravityNorm());
11835 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11836 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11837 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11838 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11839 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11840 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11841 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11842
11843
11844 calibrator = null;
11845 try {
11846 calibrator = new KnownGravityNormAccelerometerCalibrator(
11847 -gravityNorm, true, bx, by, bz, this);
11848 fail("IllegalArgumentException expected but not thrown");
11849 } catch (final IllegalArgumentException ignore) {
11850 }
11851 assertNull(calibrator);
11852 }
11853
11854 @Test
11855 public void testConstructor101() throws WrongSizeException {
11856 final Collection<StandardDeviationBodyKinematics> measurements =
11857 Collections.emptyList();
11858
11859 final Matrix ba = generateBa();
11860 final double biasX = ba.getElementAtIndex(0);
11861 final double biasY = ba.getElementAtIndex(1);
11862 final double biasZ = ba.getElementAtIndex(2);
11863
11864 final Acceleration bx = new Acceleration(biasX,
11865 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11866 final Acceleration by = new Acceleration(biasY,
11867 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11868 final Acceleration bz = new Acceleration(biasZ,
11869 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11870
11871 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11872 final double latitude = Math.toRadians(
11873 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11874 final double longitude = Math.toRadians(
11875 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11876 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11877 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11878 final NEDVelocity nedVelocity = new NEDVelocity();
11879 final ECEFPosition ecefPosition = new ECEFPosition();
11880 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11881 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11882 ecefPosition, ecefVelocity);
11883 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11884 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11885 final double gravityNorm = gravity.getNorm();
11886
11887 KnownGravityNormAccelerometerCalibrator calibrator =
11888 new KnownGravityNormAccelerometerCalibrator(
11889 gravityNorm, measurements,
11890 true, bx, by, bz);
11891
11892
11893 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11894 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11895 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11896 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11897 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11898 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11899 final Acceleration bx2 = new Acceleration(0.0,
11900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11901 calibrator.getInitialBiasXAsAcceleration(bx2);
11902 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11903 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11904 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11905 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11906 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11907 final Acceleration by2 = new Acceleration(0.0,
11908 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11909 calibrator.getInitialBiasYAsAcceleration(by2);
11910 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11911 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11912 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11913 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11914 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11915 final Acceleration bz2 = new Acceleration(0.0,
11916 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11917 calibrator.getInitialBiasZAsAcceleration(bz2);
11918 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11919 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11920 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11921 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11922 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11923 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11924 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11925 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11926 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11927 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11928 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11929 final double[] bias1 = calibrator.getInitialBias();
11930 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11931 final double[] bias2 = new double[3];
11932 calibrator.getInitialBias(bias2);
11933 assertArrayEquals(bias1, bias2, 0.0);
11934 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11935 assertEquals(b1, ba);
11936 final Matrix b2 = new Matrix(3, 1);
11937 calibrator.getInitialBiasAsMatrix(b2);
11938 assertEquals(b1, b2);
11939 final Matrix ma1 = calibrator.getInitialMa();
11940 assertEquals(ma1, new Matrix(3, 3));
11941 final Matrix ma2 = new Matrix(3, 3);
11942 calibrator.getInitialMa(ma2);
11943 assertEquals(ma1, ma2);
11944 assertSame(calibrator.getMeasurements(), measurements);
11945 assertTrue(calibrator.isCommonAxisUsed());
11946 assertNull(calibrator.getListener());
11947 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11948 assertFalse(calibrator.isReady());
11949 assertFalse(calibrator.isRunning());
11950 assertNull(calibrator.getEstimatedBiases());
11951 assertFalse(calibrator.getEstimatedBiases(null));
11952 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11953 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11954 assertNull(calibrator.getEstimatedBiasFx());
11955 assertNull(calibrator.getEstimatedBiasFy());
11956 assertNull(calibrator.getEstimatedBiasFz());
11957 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11958 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11959 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11960 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11961 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11962 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11963 assertNull(calibrator.getEstimatedMa());
11964 assertNull(calibrator.getEstimatedSx());
11965 assertNull(calibrator.getEstimatedSy());
11966 assertNull(calibrator.getEstimatedSz());
11967 assertNull(calibrator.getEstimatedMxy());
11968 assertNull(calibrator.getEstimatedMxz());
11969 assertNull(calibrator.getEstimatedMyx());
11970 assertNull(calibrator.getEstimatedMyz());
11971 assertNull(calibrator.getEstimatedMzx());
11972 assertNull(calibrator.getEstimatedMzy());
11973 assertNull(calibrator.getEstimatedCovariance());
11974 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11975 assertNotNull(calibrator.getGroundTruthGravityNorm());
11976 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11977 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11978 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11979 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11980 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11981 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11982 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11983
11984
11985 calibrator = null;
11986 try {
11987 calibrator = new KnownGravityNormAccelerometerCalibrator(
11988 -gravityNorm, measurements,
11989 true, bx, by, bz);
11990 fail("IllegalArgumentException expected but not thrown");
11991 } catch (final IllegalArgumentException ignore) {
11992 }
11993 assertNull(calibrator);
11994 }
11995
11996 @Test
11997 public void testConstructor102() throws WrongSizeException {
11998 final Collection<StandardDeviationBodyKinematics> measurements =
11999 Collections.emptyList();
12000
12001 final Matrix ba = generateBa();
12002 final double biasX = ba.getElementAtIndex(0);
12003 final double biasY = ba.getElementAtIndex(1);
12004 final double biasZ = ba.getElementAtIndex(2);
12005
12006 final Acceleration bx = new Acceleration(biasX,
12007 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12008 final Acceleration by = new Acceleration(biasY,
12009 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12010 final Acceleration bz = new Acceleration(biasZ,
12011 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12012
12013 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12014 final double latitude = Math.toRadians(
12015 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12016 final double longitude = Math.toRadians(
12017 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12018 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12019 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12020 final NEDVelocity nedVelocity = new NEDVelocity();
12021 final ECEFPosition ecefPosition = new ECEFPosition();
12022 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12023 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12024 ecefPosition, ecefVelocity);
12025 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12026 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12027 final double gravityNorm = gravity.getNorm();
12028
12029 KnownGravityNormAccelerometerCalibrator calibrator =
12030 new KnownGravityNormAccelerometerCalibrator(
12031 gravityNorm, measurements,
12032 true, bx, by, bz, this);
12033
12034
12035 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12036 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12037 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12038 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12039 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12040 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12041 final Acceleration bx2 = new Acceleration(0.0,
12042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12043 calibrator.getInitialBiasXAsAcceleration(bx2);
12044 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12045 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12046 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12047 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12048 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12049 final Acceleration by2 = new Acceleration(0.0,
12050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12051 calibrator.getInitialBiasYAsAcceleration(by2);
12052 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12053 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12054 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12055 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12056 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12057 final Acceleration bz2 = new Acceleration(0.0,
12058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12059 calibrator.getInitialBiasZAsAcceleration(bz2);
12060 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12061 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12062 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12063 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12064 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12065 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12066 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12067 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12068 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12069 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12070 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12071 final double[] bias1 = calibrator.getInitialBias();
12072 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12073 final double[] bias2 = new double[3];
12074 calibrator.getInitialBias(bias2);
12075 assertArrayEquals(bias1, bias2, 0.0);
12076 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12077 assertEquals(b1, ba);
12078 final Matrix b2 = new Matrix(3, 1);
12079 calibrator.getInitialBiasAsMatrix(b2);
12080 assertEquals(b1, b2);
12081 final Matrix ma1 = calibrator.getInitialMa();
12082 assertEquals(ma1, new Matrix(3, 3));
12083 final Matrix ma2 = new Matrix(3, 3);
12084 calibrator.getInitialMa(ma2);
12085 assertEquals(ma1, ma2);
12086 assertSame(calibrator.getMeasurements(), measurements);
12087 assertTrue(calibrator.isCommonAxisUsed());
12088 assertSame(calibrator.getListener(), this);
12089 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12090 assertFalse(calibrator.isReady());
12091 assertFalse(calibrator.isRunning());
12092 assertNull(calibrator.getEstimatedBiases());
12093 assertFalse(calibrator.getEstimatedBiases(null));
12094 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12095 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12096 assertNull(calibrator.getEstimatedBiasFx());
12097 assertNull(calibrator.getEstimatedBiasFy());
12098 assertNull(calibrator.getEstimatedBiasFz());
12099 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12100 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12101 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12102 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12103 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12104 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12105 assertNull(calibrator.getEstimatedMa());
12106 assertNull(calibrator.getEstimatedSx());
12107 assertNull(calibrator.getEstimatedSy());
12108 assertNull(calibrator.getEstimatedSz());
12109 assertNull(calibrator.getEstimatedMxy());
12110 assertNull(calibrator.getEstimatedMxz());
12111 assertNull(calibrator.getEstimatedMyx());
12112 assertNull(calibrator.getEstimatedMyz());
12113 assertNull(calibrator.getEstimatedMzx());
12114 assertNull(calibrator.getEstimatedMzy());
12115 assertNull(calibrator.getEstimatedCovariance());
12116 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12117 assertNotNull(calibrator.getGroundTruthGravityNorm());
12118 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12119 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12120 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12121 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12122 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12123 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12124 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12125
12126
12127 calibrator = null;
12128 try {
12129 calibrator = new KnownGravityNormAccelerometerCalibrator(
12130 -gravityNorm, measurements,
12131 true, bx, by, bz, this);
12132 fail("IllegalArgumentException expected but not thrown");
12133 } catch (final IllegalArgumentException ignore) {
12134 }
12135 assertNull(calibrator);
12136 }
12137
12138 @Test
12139 public void testConstructor103() throws WrongSizeException {
12140 final Matrix ba = generateBa();
12141 final double biasX = ba.getElementAtIndex(0);
12142 final double biasY = ba.getElementAtIndex(1);
12143 final double biasZ = ba.getElementAtIndex(2);
12144
12145 final Matrix ma = generateMaCommonAxis();
12146 final double sx = ma.getElementAt(0, 0);
12147 final double sy = ma.getElementAt(1, 1);
12148 final double sz = ma.getElementAt(2, 2);
12149
12150 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12151 final double latitude = Math.toRadians(
12152 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12153 final double longitude = Math.toRadians(
12154 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12155 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12156 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12157 final NEDVelocity nedVelocity = new NEDVelocity();
12158 final ECEFPosition ecefPosition = new ECEFPosition();
12159 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12160 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12161 ecefPosition, ecefVelocity);
12162 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12163 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12164 final double gravityNorm = gravity.getNorm();
12165
12166 KnownGravityNormAccelerometerCalibrator calibrator =
12167 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12168 biasX, biasY, biasZ, sx, sy, sz);
12169
12170
12171 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12172 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12173 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12174 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12175 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12176 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12177 final Acceleration bx2 = new Acceleration(0.0,
12178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12179 calibrator.getInitialBiasXAsAcceleration(bx2);
12180 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12181 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12182 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12183 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12184 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12185 final Acceleration by2 = new Acceleration(0.0,
12186 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12187 calibrator.getInitialBiasYAsAcceleration(by2);
12188 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12189 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12190 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12191 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12192 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12193 final Acceleration bz2 = new Acceleration(0.0,
12194 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12195 calibrator.getInitialBiasZAsAcceleration(bz2);
12196 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12197 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12198 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12199 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12200 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12201 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12202 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12203 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12204 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12205 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12206 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12207 final double[] bias1 = calibrator.getInitialBias();
12208 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12209 final double[] bias2 = new double[3];
12210 calibrator.getInitialBias(bias2);
12211 assertArrayEquals(bias1, bias2, 0.0);
12212 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12213 assertEquals(b1, ba);
12214 final Matrix b2 = new Matrix(3, 1);
12215 calibrator.getInitialBiasAsMatrix(b2);
12216 assertEquals(b1, b2);
12217 final Matrix ma1 = calibrator.getInitialMa();
12218 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12219 final Matrix ma2 = new Matrix(3, 3);
12220 calibrator.getInitialMa(ma2);
12221 assertEquals(ma1, ma2);
12222 assertNull(calibrator.getMeasurements());
12223 assertFalse(calibrator.isCommonAxisUsed());
12224 assertNull(calibrator.getListener());
12225 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12226 assertFalse(calibrator.isReady());
12227 assertFalse(calibrator.isRunning());
12228 assertNull(calibrator.getEstimatedBiases());
12229 assertFalse(calibrator.getEstimatedBiases(null));
12230 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12231 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12232 assertNull(calibrator.getEstimatedBiasFx());
12233 assertNull(calibrator.getEstimatedBiasFy());
12234 assertNull(calibrator.getEstimatedBiasFz());
12235 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12236 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12237 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12238 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12239 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12240 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12241 assertNull(calibrator.getEstimatedMa());
12242 assertNull(calibrator.getEstimatedSx());
12243 assertNull(calibrator.getEstimatedSy());
12244 assertNull(calibrator.getEstimatedSz());
12245 assertNull(calibrator.getEstimatedMxy());
12246 assertNull(calibrator.getEstimatedMxz());
12247 assertNull(calibrator.getEstimatedMyx());
12248 assertNull(calibrator.getEstimatedMyz());
12249 assertNull(calibrator.getEstimatedMzx());
12250 assertNull(calibrator.getEstimatedMzy());
12251 assertNull(calibrator.getEstimatedCovariance());
12252 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12253 assertNotNull(calibrator.getGroundTruthGravityNorm());
12254 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12255 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12256 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12257 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12258 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12259 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12260 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12261
12262
12263 calibrator = null;
12264 try {
12265 calibrator = new KnownGravityNormAccelerometerCalibrator(
12266 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz);
12267 fail("IllegalArgumentException expected but not thrown");
12268 } catch (final IllegalArgumentException ignore) {
12269 }
12270 assertNull(calibrator);
12271 }
12272
12273 @Test
12274 public void testConstructor104() throws WrongSizeException {
12275 final Collection<StandardDeviationBodyKinematics> measurements =
12276 Collections.emptyList();
12277
12278 final Matrix ba = generateBa();
12279 final double biasX = ba.getElementAtIndex(0);
12280 final double biasY = ba.getElementAtIndex(1);
12281 final double biasZ = ba.getElementAtIndex(2);
12282
12283 final Matrix ma = generateMaCommonAxis();
12284 final double sx = ma.getElementAt(0, 0);
12285 final double sy = ma.getElementAt(1, 1);
12286 final double sz = ma.getElementAt(2, 2);
12287
12288 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12289 final double latitude = Math.toRadians(
12290 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12291 final double longitude = Math.toRadians(
12292 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12293 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12294 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12295 final NEDVelocity nedVelocity = new NEDVelocity();
12296 final ECEFPosition ecefPosition = new ECEFPosition();
12297 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12298 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12299 ecefPosition, ecefVelocity);
12300 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12301 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12302 final double gravityNorm = gravity.getNorm();
12303
12304 KnownGravityNormAccelerometerCalibrator calibrator =
12305 new KnownGravityNormAccelerometerCalibrator(
12306 gravityNorm, measurements,
12307 biasX, biasY, biasZ, sx, sy, sz);
12308
12309
12310 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12311 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12312 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12313 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12314 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12315 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12316 final Acceleration bx2 = new Acceleration(0.0,
12317 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12318 calibrator.getInitialBiasXAsAcceleration(bx2);
12319 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12320 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12321 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12322 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12323 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12324 final Acceleration by2 = new Acceleration(0.0,
12325 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12326 calibrator.getInitialBiasYAsAcceleration(by2);
12327 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12328 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12329 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12330 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12331 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12332 final Acceleration bz2 = new Acceleration(0.0,
12333 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12334 calibrator.getInitialBiasZAsAcceleration(bz2);
12335 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12336 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12337 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12338 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12339 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12340 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12341 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12342 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12343 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12344 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12345 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12346 final double[] bias1 = calibrator.getInitialBias();
12347 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12348 final double[] bias2 = new double[3];
12349 calibrator.getInitialBias(bias2);
12350 assertArrayEquals(bias1, bias2, 0.0);
12351 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12352 assertEquals(b1, ba);
12353 final Matrix b2 = new Matrix(3, 1);
12354 calibrator.getInitialBiasAsMatrix(b2);
12355 assertEquals(b1, b2);
12356 final Matrix ma1 = calibrator.getInitialMa();
12357 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12358 final Matrix ma2 = new Matrix(3, 3);
12359 calibrator.getInitialMa(ma2);
12360 assertEquals(ma1, ma2);
12361 assertSame(calibrator.getMeasurements(), measurements);
12362 assertFalse(calibrator.isCommonAxisUsed());
12363 assertNull(calibrator.getListener());
12364 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12365 assertFalse(calibrator.isReady());
12366 assertFalse(calibrator.isRunning());
12367 assertNull(calibrator.getEstimatedBiases());
12368 assertFalse(calibrator.getEstimatedBiases(null));
12369 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12370 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12371 assertNull(calibrator.getEstimatedBiasFx());
12372 assertNull(calibrator.getEstimatedBiasFy());
12373 assertNull(calibrator.getEstimatedBiasFz());
12374 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12375 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12376 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12377 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12378 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12379 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12380 assertNull(calibrator.getEstimatedMa());
12381 assertNull(calibrator.getEstimatedSx());
12382 assertNull(calibrator.getEstimatedSy());
12383 assertNull(calibrator.getEstimatedSz());
12384 assertNull(calibrator.getEstimatedMxy());
12385 assertNull(calibrator.getEstimatedMxz());
12386 assertNull(calibrator.getEstimatedMyx());
12387 assertNull(calibrator.getEstimatedMyz());
12388 assertNull(calibrator.getEstimatedMzx());
12389 assertNull(calibrator.getEstimatedMzy());
12390 assertNull(calibrator.getEstimatedCovariance());
12391 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12392 assertNotNull(calibrator.getGroundTruthGravityNorm());
12393 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12394 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12395 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12396 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12397 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12398 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12399 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12400
12401
12402 calibrator = null;
12403 try {
12404 calibrator = new KnownGravityNormAccelerometerCalibrator(
12405 -gravityNorm, measurements,
12406 biasX, biasY, biasZ, sx, sy, sz);
12407 fail("IllegalArgumentException expected but not thrown");
12408 } catch (final IllegalArgumentException ignore) {
12409 }
12410 assertNull(calibrator);
12411 }
12412
12413 @Test
12414 public void testConstructor105() throws WrongSizeException {
12415 final Collection<StandardDeviationBodyKinematics> measurements =
12416 Collections.emptyList();
12417
12418 final Matrix ba = generateBa();
12419 final double biasX = ba.getElementAtIndex(0);
12420 final double biasY = ba.getElementAtIndex(1);
12421 final double biasZ = ba.getElementAtIndex(2);
12422
12423 final Matrix ma = generateMaCommonAxis();
12424 final double sx = ma.getElementAt(0, 0);
12425 final double sy = ma.getElementAt(1, 1);
12426 final double sz = ma.getElementAt(2, 2);
12427
12428 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12429 final double latitude = Math.toRadians(
12430 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12431 final double longitude = Math.toRadians(
12432 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12433 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12434 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12435 final NEDVelocity nedVelocity = new NEDVelocity();
12436 final ECEFPosition ecefPosition = new ECEFPosition();
12437 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12438 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12439 ecefPosition, ecefVelocity);
12440 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12441 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12442 final double gravityNorm = gravity.getNorm();
12443
12444 KnownGravityNormAccelerometerCalibrator calibrator =
12445 new KnownGravityNormAccelerometerCalibrator(
12446 gravityNorm, measurements,
12447 biasX, biasY, biasZ, sx, sy, sz, this);
12448
12449
12450 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12451 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12452 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12453 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12454 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12455 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12456 final Acceleration bx2 = new Acceleration(0.0,
12457 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12458 calibrator.getInitialBiasXAsAcceleration(bx2);
12459 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12460 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12461 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12462 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12463 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12464 final Acceleration by2 = new Acceleration(0.0,
12465 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12466 calibrator.getInitialBiasYAsAcceleration(by2);
12467 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12468 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12469 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12470 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12471 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12472 final Acceleration bz2 = new Acceleration(0.0,
12473 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12474 calibrator.getInitialBiasZAsAcceleration(bz2);
12475 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12476 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12477 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12478 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12479 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12480 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12481 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12482 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12483 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12484 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12485 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12486 final double[] bias1 = calibrator.getInitialBias();
12487 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12488 final double[] bias2 = new double[3];
12489 calibrator.getInitialBias(bias2);
12490 assertArrayEquals(bias1, bias2, 0.0);
12491 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12492 assertEquals(b1, ba);
12493 final Matrix b2 = new Matrix(3, 1);
12494 calibrator.getInitialBiasAsMatrix(b2);
12495 assertEquals(b1, b2);
12496 final Matrix ma1 = calibrator.getInitialMa();
12497 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12498 final Matrix ma2 = new Matrix(3, 3);
12499 calibrator.getInitialMa(ma2);
12500 assertEquals(ma1, ma2);
12501 assertSame(calibrator.getMeasurements(), measurements);
12502 assertFalse(calibrator.isCommonAxisUsed());
12503 assertSame(calibrator.getListener(), this);
12504 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12505 assertFalse(calibrator.isReady());
12506 assertFalse(calibrator.isRunning());
12507 assertNull(calibrator.getEstimatedBiases());
12508 assertFalse(calibrator.getEstimatedBiases(null));
12509 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12510 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12511 assertNull(calibrator.getEstimatedBiasFx());
12512 assertNull(calibrator.getEstimatedBiasFy());
12513 assertNull(calibrator.getEstimatedBiasFz());
12514 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12515 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12516 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12517 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12518 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12519 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12520 assertNull(calibrator.getEstimatedMa());
12521 assertNull(calibrator.getEstimatedSx());
12522 assertNull(calibrator.getEstimatedSy());
12523 assertNull(calibrator.getEstimatedSz());
12524 assertNull(calibrator.getEstimatedMxy());
12525 assertNull(calibrator.getEstimatedMxz());
12526 assertNull(calibrator.getEstimatedMyx());
12527 assertNull(calibrator.getEstimatedMyz());
12528 assertNull(calibrator.getEstimatedMzx());
12529 assertNull(calibrator.getEstimatedMzy());
12530 assertNull(calibrator.getEstimatedCovariance());
12531 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12532 assertNotNull(calibrator.getGroundTruthGravityNorm());
12533 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12534 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12535 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12536 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12537 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12538 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12539 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12540
12541
12542 calibrator = null;
12543 try {
12544 calibrator = new KnownGravityNormAccelerometerCalibrator(
12545 -gravityNorm, measurements,
12546 biasX, biasY, biasZ, sx, sy, sz, this);
12547 fail("IllegalArgumentException expected but not thrown");
12548 } catch (final IllegalArgumentException ignore) {
12549 }
12550 assertNull(calibrator);
12551 }
12552
12553 @Test
12554 public void testConstructor106() throws WrongSizeException {
12555 final Matrix ba = generateBa();
12556 final double biasX = ba.getElementAtIndex(0);
12557 final double biasY = ba.getElementAtIndex(1);
12558 final double biasZ = ba.getElementAtIndex(2);
12559
12560 final Matrix ma = generateMaCommonAxis();
12561 final double sx = ma.getElementAt(0, 0);
12562 final double sy = ma.getElementAt(1, 1);
12563 final double sz = ma.getElementAt(2, 2);
12564
12565 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12566 final double latitude = Math.toRadians(
12567 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12568 final double longitude = Math.toRadians(
12569 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12570 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12571 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12572 final NEDVelocity nedVelocity = new NEDVelocity();
12573 final ECEFPosition ecefPosition = new ECEFPosition();
12574 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12575 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12576 ecefPosition, ecefVelocity);
12577 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12578 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12579 final double gravityNorm = gravity.getNorm();
12580
12581 KnownGravityNormAccelerometerCalibrator calibrator =
12582 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12583 true, biasX, biasY, biasZ,
12584 sx, sy, sz);
12585
12586
12587 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12588 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12589 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12590 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12591 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12592 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12593 final Acceleration bx2 = new Acceleration(0.0,
12594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12595 calibrator.getInitialBiasXAsAcceleration(bx2);
12596 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12597 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12598 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12599 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12600 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12601 final Acceleration by2 = new Acceleration(0.0,
12602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12603 calibrator.getInitialBiasYAsAcceleration(by2);
12604 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12605 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12606 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12607 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12608 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12609 final Acceleration bz2 = new Acceleration(0.0,
12610 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12611 calibrator.getInitialBiasZAsAcceleration(bz2);
12612 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12613 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12614 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12615 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12616 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12617 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12618 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12619 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12620 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12621 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12622 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12623 final double[] bias1 = calibrator.getInitialBias();
12624 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12625 final double[] bias2 = new double[3];
12626 calibrator.getInitialBias(bias2);
12627 assertArrayEquals(bias1, bias2, 0.0);
12628 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12629 assertEquals(b1, ba);
12630 final Matrix b2 = new Matrix(3, 1);
12631 calibrator.getInitialBiasAsMatrix(b2);
12632 assertEquals(b1, b2);
12633 final Matrix ma1 = calibrator.getInitialMa();
12634 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12635 final Matrix ma2 = new Matrix(3, 3);
12636 calibrator.getInitialMa(ma2);
12637 assertEquals(ma1, ma2);
12638 assertNull(calibrator.getMeasurements());
12639 assertTrue(calibrator.isCommonAxisUsed());
12640 assertNull(calibrator.getListener());
12641 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12642 assertFalse(calibrator.isReady());
12643 assertFalse(calibrator.isRunning());
12644 assertNull(calibrator.getEstimatedBiases());
12645 assertFalse(calibrator.getEstimatedBiases(null));
12646 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12647 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12648 assertNull(calibrator.getEstimatedBiasFx());
12649 assertNull(calibrator.getEstimatedBiasFy());
12650 assertNull(calibrator.getEstimatedBiasFz());
12651 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12652 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12653 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12654 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12655 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12656 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12657 assertNull(calibrator.getEstimatedMa());
12658 assertNull(calibrator.getEstimatedSx());
12659 assertNull(calibrator.getEstimatedSy());
12660 assertNull(calibrator.getEstimatedSz());
12661 assertNull(calibrator.getEstimatedMxy());
12662 assertNull(calibrator.getEstimatedMxz());
12663 assertNull(calibrator.getEstimatedMyx());
12664 assertNull(calibrator.getEstimatedMyz());
12665 assertNull(calibrator.getEstimatedMzx());
12666 assertNull(calibrator.getEstimatedMzy());
12667 assertNull(calibrator.getEstimatedCovariance());
12668 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12669 assertNotNull(calibrator.getGroundTruthGravityNorm());
12670 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12671 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12672 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12673 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12674 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12675 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12676 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12677
12678
12679 calibrator = null;
12680 try {
12681 calibrator = new KnownGravityNormAccelerometerCalibrator(
12682 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz);
12683 fail("IllegalArgumentException expected but not thrown");
12684 } catch (final IllegalArgumentException ignore) {
12685 }
12686 assertNull(calibrator);
12687 }
12688
12689 @Test
12690 public void testConstructor107() throws WrongSizeException {
12691 final Matrix ba = generateBa();
12692 final double biasX = ba.getElementAtIndex(0);
12693 final double biasY = ba.getElementAtIndex(1);
12694 final double biasZ = ba.getElementAtIndex(2);
12695
12696 final Matrix ma = generateMaCommonAxis();
12697 final double sx = ma.getElementAt(0, 0);
12698 final double sy = ma.getElementAt(1, 1);
12699 final double sz = ma.getElementAt(2, 2);
12700
12701 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12702 final double latitude = Math.toRadians(
12703 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12704 final double longitude = Math.toRadians(
12705 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12706 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12707 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12708 final NEDVelocity nedVelocity = new NEDVelocity();
12709 final ECEFPosition ecefPosition = new ECEFPosition();
12710 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12711 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12712 ecefPosition, ecefVelocity);
12713 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12714 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12715 final double gravityNorm = gravity.getNorm();
12716
12717 KnownGravityNormAccelerometerCalibrator calibrator =
12718 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12719 true, biasX, biasY, biasZ, sx, sy, sz,
12720 this);
12721
12722
12723 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12724 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12725 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12726 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12727 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12728 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12729 final Acceleration bx2 = new Acceleration(0.0,
12730 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12731 calibrator.getInitialBiasXAsAcceleration(bx2);
12732 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12733 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12734 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12735 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12736 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12737 final Acceleration by2 = new Acceleration(0.0,
12738 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12739 calibrator.getInitialBiasYAsAcceleration(by2);
12740 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12741 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12742 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12743 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12744 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12745 final Acceleration bz2 = new Acceleration(0.0,
12746 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12747 calibrator.getInitialBiasZAsAcceleration(bz2);
12748 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12749 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12750 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12751 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12752 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12753 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12754 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12755 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12756 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12757 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12758 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12759 final double[] bias1 = calibrator.getInitialBias();
12760 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12761 final double[] bias2 = new double[3];
12762 calibrator.getInitialBias(bias2);
12763 assertArrayEquals(bias1, bias2, 0.0);
12764 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12765 assertEquals(b1, ba);
12766 final Matrix b2 = new Matrix(3, 1);
12767 calibrator.getInitialBiasAsMatrix(b2);
12768 assertEquals(b1, b2);
12769 final Matrix ma1 = calibrator.getInitialMa();
12770 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12771 final Matrix ma2 = new Matrix(3, 3);
12772 calibrator.getInitialMa(ma2);
12773 assertEquals(ma1, ma2);
12774 assertNull(calibrator.getMeasurements());
12775 assertTrue(calibrator.isCommonAxisUsed());
12776 assertSame(calibrator.getListener(), this);
12777 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12778 assertFalse(calibrator.isReady());
12779 assertFalse(calibrator.isRunning());
12780 assertNull(calibrator.getEstimatedBiases());
12781 assertFalse(calibrator.getEstimatedBiases(null));
12782 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12783 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12784 assertNull(calibrator.getEstimatedBiasFx());
12785 assertNull(calibrator.getEstimatedBiasFy());
12786 assertNull(calibrator.getEstimatedBiasFz());
12787 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12788 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12789 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12790 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12791 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12792 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12793 assertNull(calibrator.getEstimatedMa());
12794 assertNull(calibrator.getEstimatedSx());
12795 assertNull(calibrator.getEstimatedSy());
12796 assertNull(calibrator.getEstimatedSz());
12797 assertNull(calibrator.getEstimatedMxy());
12798 assertNull(calibrator.getEstimatedMxz());
12799 assertNull(calibrator.getEstimatedMyx());
12800 assertNull(calibrator.getEstimatedMyz());
12801 assertNull(calibrator.getEstimatedMzx());
12802 assertNull(calibrator.getEstimatedMzy());
12803 assertNull(calibrator.getEstimatedCovariance());
12804 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12805 assertNotNull(calibrator.getGroundTruthGravityNorm());
12806 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12807 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12808 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12809 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12810 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12811 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12812 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12813
12814
12815 calibrator = null;
12816 try {
12817 calibrator = new KnownGravityNormAccelerometerCalibrator(
12818 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
12819 this);
12820 fail("IllegalArgumentException expected but not thrown");
12821 } catch (final IllegalArgumentException ignore) {
12822 }
12823 assertNull(calibrator);
12824 }
12825
12826 @Test
12827 public void testConstructor108() throws WrongSizeException {
12828 final Collection<StandardDeviationBodyKinematics> measurements =
12829 Collections.emptyList();
12830
12831 final Matrix ba = generateBa();
12832 final double biasX = ba.getElementAtIndex(0);
12833 final double biasY = ba.getElementAtIndex(1);
12834 final double biasZ = ba.getElementAtIndex(2);
12835
12836 final Matrix ma = generateMaCommonAxis();
12837 final double sx = ma.getElementAt(0, 0);
12838 final double sy = ma.getElementAt(1, 1);
12839 final double sz = ma.getElementAt(2, 2);
12840
12841 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12842 final double latitude = Math.toRadians(
12843 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12844 final double longitude = Math.toRadians(
12845 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12846 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12847 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12848 final NEDVelocity nedVelocity = new NEDVelocity();
12849 final ECEFPosition ecefPosition = new ECEFPosition();
12850 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12851 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12852 ecefPosition, ecefVelocity);
12853 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12854 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12855 final double gravityNorm = gravity.getNorm();
12856
12857 KnownGravityNormAccelerometerCalibrator calibrator =
12858 new KnownGravityNormAccelerometerCalibrator(
12859 gravityNorm, measurements,
12860 true, biasX, biasY, biasZ,
12861 sx, sy, sz);
12862
12863
12864 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12865 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12866 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12867 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12868 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12869 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12870 final Acceleration bx2 = new Acceleration(0.0,
12871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12872 calibrator.getInitialBiasXAsAcceleration(bx2);
12873 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12874 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12875 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12876 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12877 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12878 final Acceleration by2 = new Acceleration(0.0,
12879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12880 calibrator.getInitialBiasYAsAcceleration(by2);
12881 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12882 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12883 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12884 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12885 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12886 final Acceleration bz2 = new Acceleration(0.0,
12887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12888 calibrator.getInitialBiasZAsAcceleration(bz2);
12889 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12890 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12891 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12892 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12893 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12894 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12895 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12896 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12897 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12898 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12899 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12900 final double[] bias1 = calibrator.getInitialBias();
12901 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12902 final double[] bias2 = new double[3];
12903 calibrator.getInitialBias(bias2);
12904 assertArrayEquals(bias1, bias2, 0.0);
12905 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12906 assertEquals(b1, ba);
12907 final Matrix b2 = new Matrix(3, 1);
12908 calibrator.getInitialBiasAsMatrix(b2);
12909 assertEquals(b1, b2);
12910 final Matrix ma1 = calibrator.getInitialMa();
12911 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12912 final Matrix ma2 = new Matrix(3, 3);
12913 calibrator.getInitialMa(ma2);
12914 assertEquals(ma1, ma2);
12915 assertSame(calibrator.getMeasurements(), measurements);
12916 assertTrue(calibrator.isCommonAxisUsed());
12917 assertNull(calibrator.getListener());
12918 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12919 assertFalse(calibrator.isReady());
12920 assertFalse(calibrator.isRunning());
12921 assertNull(calibrator.getEstimatedBiases());
12922 assertFalse(calibrator.getEstimatedBiases(null));
12923 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12924 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12925 assertNull(calibrator.getEstimatedBiasFx());
12926 assertNull(calibrator.getEstimatedBiasFy());
12927 assertNull(calibrator.getEstimatedBiasFz());
12928 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12929 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12930 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12931 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12932 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12933 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12934 assertNull(calibrator.getEstimatedMa());
12935 assertNull(calibrator.getEstimatedSx());
12936 assertNull(calibrator.getEstimatedSy());
12937 assertNull(calibrator.getEstimatedSz());
12938 assertNull(calibrator.getEstimatedMxy());
12939 assertNull(calibrator.getEstimatedMxz());
12940 assertNull(calibrator.getEstimatedMyx());
12941 assertNull(calibrator.getEstimatedMyz());
12942 assertNull(calibrator.getEstimatedMzx());
12943 assertNull(calibrator.getEstimatedMzy());
12944 assertNull(calibrator.getEstimatedCovariance());
12945 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12946 assertNotNull(calibrator.getGroundTruthGravityNorm());
12947 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12948 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12949 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12950 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12951 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12952 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12953 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12954
12955
12956 calibrator = null;
12957 try {
12958 calibrator = new KnownGravityNormAccelerometerCalibrator(
12959 -gravityNorm, measurements,
12960 true, biasX, biasY, biasZ,
12961 sx, sy, sz);
12962 fail("IllegalArgumentException expected but not thrown");
12963 } catch (final IllegalArgumentException ignore) {
12964 }
12965 assertNull(calibrator);
12966 }
12967
12968 @Test
12969 public void testConstructor109() throws WrongSizeException {
12970 final Collection<StandardDeviationBodyKinematics> measurements =
12971 Collections.emptyList();
12972
12973 final Matrix ba = generateBa();
12974 final double biasX = ba.getElementAtIndex(0);
12975 final double biasY = ba.getElementAtIndex(1);
12976 final double biasZ = ba.getElementAtIndex(2);
12977
12978 final Matrix ma = generateMaCommonAxis();
12979 final double sx = ma.getElementAt(0, 0);
12980 final double sy = ma.getElementAt(1, 1);
12981 final double sz = ma.getElementAt(2, 2);
12982
12983 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12984 final double latitude = Math.toRadians(
12985 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12986 final double longitude = Math.toRadians(
12987 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12988 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12989 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12990 final NEDVelocity nedVelocity = new NEDVelocity();
12991 final ECEFPosition ecefPosition = new ECEFPosition();
12992 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12993 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12994 ecefPosition, ecefVelocity);
12995 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12996 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12997 final double gravityNorm = gravity.getNorm();
12998
12999 KnownGravityNormAccelerometerCalibrator calibrator =
13000 new KnownGravityNormAccelerometerCalibrator(
13001 gravityNorm, measurements,
13002 true, biasX, biasY, biasZ, sx, sy, sz,
13003 this);
13004
13005
13006 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13007 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13008 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13009 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13010 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13011 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13012 final Acceleration bx2 = new Acceleration(0.0,
13013 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13014 calibrator.getInitialBiasXAsAcceleration(bx2);
13015 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13016 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13017 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13018 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13019 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13020 final Acceleration by2 = new Acceleration(0.0,
13021 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13022 calibrator.getInitialBiasYAsAcceleration(by2);
13023 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13024 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13025 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13026 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13027 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13028 final Acceleration bz2 = new Acceleration(0.0,
13029 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13030 calibrator.getInitialBiasZAsAcceleration(bz2);
13031 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13032 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13033 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13034 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13035 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13036 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13037 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13038 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13039 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13040 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13041 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13042 final double[] bias1 = calibrator.getInitialBias();
13043 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13044 final double[] bias2 = new double[3];
13045 calibrator.getInitialBias(bias2);
13046 assertArrayEquals(bias1, bias2, 0.0);
13047 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13048 assertEquals(b1, ba);
13049 final Matrix b2 = new Matrix(3, 1);
13050 calibrator.getInitialBiasAsMatrix(b2);
13051 assertEquals(b1, b2);
13052 final Matrix ma1 = calibrator.getInitialMa();
13053 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13054 final Matrix ma2 = new Matrix(3, 3);
13055 calibrator.getInitialMa(ma2);
13056 assertEquals(ma1, ma2);
13057 assertSame(calibrator.getMeasurements(), measurements);
13058 assertTrue(calibrator.isCommonAxisUsed());
13059 assertSame(calibrator.getListener(), this);
13060 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13061 assertFalse(calibrator.isReady());
13062 assertFalse(calibrator.isRunning());
13063 assertNull(calibrator.getEstimatedBiases());
13064 assertFalse(calibrator.getEstimatedBiases(null));
13065 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13066 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13067 assertNull(calibrator.getEstimatedBiasFx());
13068 assertNull(calibrator.getEstimatedBiasFy());
13069 assertNull(calibrator.getEstimatedBiasFz());
13070 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13071 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13072 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13073 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13074 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13075 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13076 assertNull(calibrator.getEstimatedMa());
13077 assertNull(calibrator.getEstimatedSx());
13078 assertNull(calibrator.getEstimatedSy());
13079 assertNull(calibrator.getEstimatedSz());
13080 assertNull(calibrator.getEstimatedMxy());
13081 assertNull(calibrator.getEstimatedMxz());
13082 assertNull(calibrator.getEstimatedMyx());
13083 assertNull(calibrator.getEstimatedMyz());
13084 assertNull(calibrator.getEstimatedMzx());
13085 assertNull(calibrator.getEstimatedMzy());
13086 assertNull(calibrator.getEstimatedCovariance());
13087 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13088 assertNotNull(calibrator.getGroundTruthGravityNorm());
13089 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13090 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13091 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13092 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13093 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13094 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13095 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13096
13097
13098 calibrator = null;
13099 try {
13100 calibrator = new KnownGravityNormAccelerometerCalibrator(
13101 -gravityNorm, measurements,
13102 true, biasX, biasY, biasZ, sx, sy, sz,
13103 this);
13104 fail("IllegalArgumentException expected but not thrown");
13105 } catch (final IllegalArgumentException ignore) {
13106 }
13107 assertNull(calibrator);
13108 }
13109
13110 @Test
13111 public void testConstructor110() throws WrongSizeException {
13112 final Matrix ba = generateBa();
13113 final double biasX = ba.getElementAtIndex(0);
13114 final double biasY = ba.getElementAtIndex(1);
13115 final double biasZ = ba.getElementAtIndex(2);
13116
13117 final Acceleration bx = new Acceleration(biasX,
13118 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13119 final Acceleration by = new Acceleration(biasY,
13120 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13121 final Acceleration bz = new Acceleration(biasZ,
13122 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13123
13124 final Matrix ma = generateMaCommonAxis();
13125 final double sx = ma.getElementAt(0, 0);
13126 final double sy = ma.getElementAt(1, 1);
13127 final double sz = ma.getElementAt(2, 2);
13128
13129 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13130 final double latitude = Math.toRadians(
13131 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13132 final double longitude = Math.toRadians(
13133 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13134 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13135 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13136 final NEDVelocity nedVelocity = new NEDVelocity();
13137 final ECEFPosition ecefPosition = new ECEFPosition();
13138 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13139 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13140 ecefPosition, ecefVelocity);
13141 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13142 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13143 final double gravityNorm = gravity.getNorm();
13144
13145 KnownGravityNormAccelerometerCalibrator calibrator =
13146 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13147 bx, by, bz, sx, sy, sz);
13148
13149
13150 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13151 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13152 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13153 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13154 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13155 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13156 final Acceleration bx2 = new Acceleration(0.0,
13157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13158 calibrator.getInitialBiasXAsAcceleration(bx2);
13159 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13160 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13161 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13162 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13163 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13164 final Acceleration by2 = new Acceleration(0.0,
13165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13166 calibrator.getInitialBiasYAsAcceleration(by2);
13167 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13168 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13169 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13170 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13171 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13172 final Acceleration bz2 = new Acceleration(0.0,
13173 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13174 calibrator.getInitialBiasZAsAcceleration(bz2);
13175 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13176 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13177 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13178 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13179 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13180 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13181 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13182 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13183 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13184 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13185 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13186 final double[] bias1 = calibrator.getInitialBias();
13187 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13188 final double[] bias2 = new double[3];
13189 calibrator.getInitialBias(bias2);
13190 assertArrayEquals(bias1, bias2, 0.0);
13191 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13192 assertEquals(b1, ba);
13193 final Matrix b2 = new Matrix(3, 1);
13194 calibrator.getInitialBiasAsMatrix(b2);
13195 assertEquals(b1, b2);
13196 final Matrix ma1 = calibrator.getInitialMa();
13197 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13198 final Matrix ma2 = new Matrix(3, 3);
13199 calibrator.getInitialMa(ma2);
13200 assertEquals(ma1, ma2);
13201 assertNull(calibrator.getMeasurements());
13202 assertFalse(calibrator.isCommonAxisUsed());
13203 assertNull(calibrator.getListener());
13204 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13205 assertFalse(calibrator.isReady());
13206 assertFalse(calibrator.isRunning());
13207 assertNull(calibrator.getEstimatedBiases());
13208 assertFalse(calibrator.getEstimatedBiases(null));
13209 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13210 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13211 assertNull(calibrator.getEstimatedBiasFx());
13212 assertNull(calibrator.getEstimatedBiasFy());
13213 assertNull(calibrator.getEstimatedBiasFz());
13214 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13215 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13216 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13217 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13218 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13219 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13220 assertNull(calibrator.getEstimatedMa());
13221 assertNull(calibrator.getEstimatedSx());
13222 assertNull(calibrator.getEstimatedSy());
13223 assertNull(calibrator.getEstimatedSz());
13224 assertNull(calibrator.getEstimatedMxy());
13225 assertNull(calibrator.getEstimatedMxz());
13226 assertNull(calibrator.getEstimatedMyx());
13227 assertNull(calibrator.getEstimatedMyz());
13228 assertNull(calibrator.getEstimatedMzx());
13229 assertNull(calibrator.getEstimatedMzy());
13230 assertNull(calibrator.getEstimatedCovariance());
13231 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13232 assertNotNull(calibrator.getGroundTruthGravityNorm());
13233 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13234 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13235 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13236 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13237 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13238 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13239 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13240
13241
13242 calibrator = null;
13243 try {
13244 calibrator = new KnownGravityNormAccelerometerCalibrator(
13245 -gravityNorm, bx, by, bz, sx, sy, sz);
13246 fail("IllegalArgumentException expected but not thrown");
13247 } catch (final IllegalArgumentException ignore) {
13248 }
13249 assertNull(calibrator);
13250 }
13251
13252 @Test
13253 public void testConstructor111() throws WrongSizeException {
13254 final Matrix ba = generateBa();
13255 final double biasX = ba.getElementAtIndex(0);
13256 final double biasY = ba.getElementAtIndex(1);
13257 final double biasZ = ba.getElementAtIndex(2);
13258
13259 final Acceleration bx = new Acceleration(biasX,
13260 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13261 final Acceleration by = new Acceleration(biasY,
13262 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13263 final Acceleration bz = new Acceleration(biasZ,
13264 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13265
13266 final Matrix ma = generateMaCommonAxis();
13267 final double sx = ma.getElementAt(0, 0);
13268 final double sy = ma.getElementAt(1, 1);
13269 final double sz = ma.getElementAt(2, 2);
13270
13271 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13272 final double latitude = Math.toRadians(
13273 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13274 final double longitude = Math.toRadians(
13275 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13276 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13277 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13278 final NEDVelocity nedVelocity = new NEDVelocity();
13279 final ECEFPosition ecefPosition = new ECEFPosition();
13280 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13281 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13282 ecefPosition, ecefVelocity);
13283 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13284 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13285 final double gravityNorm = gravity.getNorm();
13286
13287 KnownGravityNormAccelerometerCalibrator calibrator =
13288 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13289 bx, by, bz, sx, sy, sz, this);
13290
13291
13292 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13293 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13294 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13295 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13296 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13297 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13298 final Acceleration bx2 = new Acceleration(0.0,
13299 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13300 calibrator.getInitialBiasXAsAcceleration(bx2);
13301 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13302 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13303 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13304 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13305 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13306 final Acceleration by2 = new Acceleration(0.0,
13307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13308 calibrator.getInitialBiasYAsAcceleration(by2);
13309 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13310 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13311 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13312 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13313 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13314 final Acceleration bz2 = new Acceleration(0.0,
13315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13316 calibrator.getInitialBiasZAsAcceleration(bz2);
13317 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13318 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13319 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13320 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13321 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13322 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13323 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13324 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13325 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13326 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13327 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13328 final double[] bias1 = calibrator.getInitialBias();
13329 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13330 final double[] bias2 = new double[3];
13331 calibrator.getInitialBias(bias2);
13332 assertArrayEquals(bias1, bias2, 0.0);
13333 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13334 assertEquals(b1, ba);
13335 final Matrix b2 = new Matrix(3, 1);
13336 calibrator.getInitialBiasAsMatrix(b2);
13337 assertEquals(b1, b2);
13338 final Matrix ma1 = calibrator.getInitialMa();
13339 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13340 final Matrix ma2 = new Matrix(3, 3);
13341 calibrator.getInitialMa(ma2);
13342 assertEquals(ma1, ma2);
13343 assertNull(calibrator.getMeasurements());
13344 assertFalse(calibrator.isCommonAxisUsed());
13345 assertSame(calibrator.getListener(), this);
13346 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13347 assertFalse(calibrator.isReady());
13348 assertFalse(calibrator.isRunning());
13349 assertNull(calibrator.getEstimatedBiases());
13350 assertFalse(calibrator.getEstimatedBiases(null));
13351 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13352 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13353 assertNull(calibrator.getEstimatedBiasFx());
13354 assertNull(calibrator.getEstimatedBiasFy());
13355 assertNull(calibrator.getEstimatedBiasFz());
13356 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13357 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13358 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13359 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13360 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13361 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13362 assertNull(calibrator.getEstimatedMa());
13363 assertNull(calibrator.getEstimatedSx());
13364 assertNull(calibrator.getEstimatedSy());
13365 assertNull(calibrator.getEstimatedSz());
13366 assertNull(calibrator.getEstimatedMxy());
13367 assertNull(calibrator.getEstimatedMxz());
13368 assertNull(calibrator.getEstimatedMyx());
13369 assertNull(calibrator.getEstimatedMyz());
13370 assertNull(calibrator.getEstimatedMzx());
13371 assertNull(calibrator.getEstimatedMzy());
13372 assertNull(calibrator.getEstimatedCovariance());
13373 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13374 assertNotNull(calibrator.getGroundTruthGravityNorm());
13375 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13376 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13377 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13378 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13379 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13380 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13381 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13382
13383
13384 calibrator = null;
13385 try {
13386 calibrator = new KnownGravityNormAccelerometerCalibrator(
13387 -gravityNorm, bx, by, bz, sx, sy, sz, this);
13388 fail("IllegalArgumentException expected but not thrown");
13389 } catch (final IllegalArgumentException ignore) {
13390 }
13391 assertNull(calibrator);
13392 }
13393
13394 @Test
13395 public void testConstructor112() throws WrongSizeException {
13396 final Collection<StandardDeviationBodyKinematics> measurements =
13397 Collections.emptyList();
13398
13399 final Matrix ba = generateBa();
13400 final double biasX = ba.getElementAtIndex(0);
13401 final double biasY = ba.getElementAtIndex(1);
13402 final double biasZ = ba.getElementAtIndex(2);
13403
13404 final Acceleration bx = new Acceleration(biasX,
13405 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13406 final Acceleration by = new Acceleration(biasY,
13407 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13408 final Acceleration bz = new Acceleration(biasZ,
13409 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13410
13411 final Matrix ma = generateMaCommonAxis();
13412 final double sx = ma.getElementAt(0, 0);
13413 final double sy = ma.getElementAt(1, 1);
13414 final double sz = ma.getElementAt(2, 2);
13415
13416 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13417 final double latitude = Math.toRadians(
13418 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13419 final double longitude = Math.toRadians(
13420 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13421 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13422 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13423 final NEDVelocity nedVelocity = new NEDVelocity();
13424 final ECEFPosition ecefPosition = new ECEFPosition();
13425 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13426 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13427 ecefPosition, ecefVelocity);
13428 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13429 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13430 final double gravityNorm = gravity.getNorm();
13431
13432 KnownGravityNormAccelerometerCalibrator calibrator =
13433 new KnownGravityNormAccelerometerCalibrator(
13434 gravityNorm, measurements,
13435 bx, by, bz, sx, sy, sz);
13436
13437
13438 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13439 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13440 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13441 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13442 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13443 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13444 final Acceleration bx2 = new Acceleration(0.0,
13445 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13446 calibrator.getInitialBiasXAsAcceleration(bx2);
13447 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13448 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13449 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13450 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13451 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13452 final Acceleration by2 = new Acceleration(0.0,
13453 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13454 calibrator.getInitialBiasYAsAcceleration(by2);
13455 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13456 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13457 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13458 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13459 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13460 final Acceleration bz2 = new Acceleration(0.0,
13461 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13462 calibrator.getInitialBiasZAsAcceleration(bz2);
13463 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13464 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13465 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13466 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13467 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13468 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13469 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13470 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13471 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13472 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13473 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13474 final double[] bias1 = calibrator.getInitialBias();
13475 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13476 final double[] bias2 = new double[3];
13477 calibrator.getInitialBias(bias2);
13478 assertArrayEquals(bias1, bias2, 0.0);
13479 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13480 assertEquals(b1, ba);
13481 final Matrix b2 = new Matrix(3, 1);
13482 calibrator.getInitialBiasAsMatrix(b2);
13483 assertEquals(b1, b2);
13484 final Matrix ma1 = calibrator.getInitialMa();
13485 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13486 final Matrix ma2 = new Matrix(3, 3);
13487 calibrator.getInitialMa(ma2);
13488 assertEquals(ma1, ma2);
13489 assertSame(calibrator.getMeasurements(), measurements);
13490 assertFalse(calibrator.isCommonAxisUsed());
13491 assertNull(calibrator.getListener());
13492 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13493 assertFalse(calibrator.isReady());
13494 assertFalse(calibrator.isRunning());
13495 assertNull(calibrator.getEstimatedBiases());
13496 assertFalse(calibrator.getEstimatedBiases(null));
13497 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13498 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13499 assertNull(calibrator.getEstimatedBiasFx());
13500 assertNull(calibrator.getEstimatedBiasFy());
13501 assertNull(calibrator.getEstimatedBiasFz());
13502 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13503 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13504 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13505 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13506 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13507 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13508 assertNull(calibrator.getEstimatedMa());
13509 assertNull(calibrator.getEstimatedSx());
13510 assertNull(calibrator.getEstimatedSy());
13511 assertNull(calibrator.getEstimatedSz());
13512 assertNull(calibrator.getEstimatedMxy());
13513 assertNull(calibrator.getEstimatedMxz());
13514 assertNull(calibrator.getEstimatedMyx());
13515 assertNull(calibrator.getEstimatedMyz());
13516 assertNull(calibrator.getEstimatedMzx());
13517 assertNull(calibrator.getEstimatedMzy());
13518 assertNull(calibrator.getEstimatedCovariance());
13519 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13520 assertNotNull(calibrator.getGroundTruthGravityNorm());
13521 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13522 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13523 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13524 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13525 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13526 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13527 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13528
13529
13530 calibrator = null;
13531 try {
13532 calibrator = new KnownGravityNormAccelerometerCalibrator(
13533 -gravityNorm, measurements,
13534 bx, by, bz, sx, sy, sz);
13535 fail("IllegalArgumentException expected but not thrown");
13536 } catch (final IllegalArgumentException ignore) {
13537 }
13538 assertNull(calibrator);
13539 }
13540
13541 @Test
13542 public void testConstructor113() throws WrongSizeException {
13543 final Collection<StandardDeviationBodyKinematics> measurements =
13544 Collections.emptyList();
13545
13546 final Matrix ba = generateBa();
13547 final double biasX = ba.getElementAtIndex(0);
13548 final double biasY = ba.getElementAtIndex(1);
13549 final double biasZ = ba.getElementAtIndex(2);
13550
13551 final Acceleration bx = new Acceleration(biasX,
13552 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13553 final Acceleration by = new Acceleration(biasY,
13554 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13555 final Acceleration bz = new Acceleration(biasZ,
13556 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13557
13558 final Matrix ma = generateMaCommonAxis();
13559 final double sx = ma.getElementAt(0, 0);
13560 final double sy = ma.getElementAt(1, 1);
13561 final double sz = ma.getElementAt(2, 2);
13562
13563 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13564 final double latitude = Math.toRadians(
13565 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13566 final double longitude = Math.toRadians(
13567 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13568 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13569 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13570 final NEDVelocity nedVelocity = new NEDVelocity();
13571 final ECEFPosition ecefPosition = new ECEFPosition();
13572 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13573 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13574 ecefPosition, ecefVelocity);
13575 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13576 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13577 final double gravityNorm = gravity.getNorm();
13578
13579 KnownGravityNormAccelerometerCalibrator calibrator =
13580 new KnownGravityNormAccelerometerCalibrator(
13581 gravityNorm, measurements,
13582 bx, by, bz, sx, sy, sz, this);
13583
13584
13585 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13586 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13587 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13588 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13589 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13590 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13591 final Acceleration bx2 = new Acceleration(0.0,
13592 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13593 calibrator.getInitialBiasXAsAcceleration(bx2);
13594 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13595 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13596 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13597 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13598 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13599 final Acceleration by2 = new Acceleration(0.0,
13600 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13601 calibrator.getInitialBiasYAsAcceleration(by2);
13602 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13603 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13604 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13605 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13606 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13607 final Acceleration bz2 = new Acceleration(0.0,
13608 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13609 calibrator.getInitialBiasZAsAcceleration(bz2);
13610 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13611 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13612 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13613 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13614 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13615 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13616 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13617 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13618 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13619 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13620 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13621 final double[] bias1 = calibrator.getInitialBias();
13622 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13623 final double[] bias2 = new double[3];
13624 calibrator.getInitialBias(bias2);
13625 assertArrayEquals(bias1, bias2, 0.0);
13626 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13627 assertEquals(b1, ba);
13628 final Matrix b2 = new Matrix(3, 1);
13629 calibrator.getInitialBiasAsMatrix(b2);
13630 assertEquals(b1, b2);
13631 final Matrix ma1 = calibrator.getInitialMa();
13632 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13633 final Matrix ma2 = new Matrix(3, 3);
13634 calibrator.getInitialMa(ma2);
13635 assertEquals(ma1, ma2);
13636 assertSame(calibrator.getMeasurements(), measurements);
13637 assertFalse(calibrator.isCommonAxisUsed());
13638 assertSame(calibrator.getListener(), this);
13639 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13640 assertFalse(calibrator.isReady());
13641 assertFalse(calibrator.isRunning());
13642 assertNull(calibrator.getEstimatedBiases());
13643 assertFalse(calibrator.getEstimatedBiases(null));
13644 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13645 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13646 assertNull(calibrator.getEstimatedBiasFx());
13647 assertNull(calibrator.getEstimatedBiasFy());
13648 assertNull(calibrator.getEstimatedBiasFz());
13649 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13650 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13651 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13652 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13653 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13654 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13655 assertNull(calibrator.getEstimatedMa());
13656 assertNull(calibrator.getEstimatedSx());
13657 assertNull(calibrator.getEstimatedSy());
13658 assertNull(calibrator.getEstimatedSz());
13659 assertNull(calibrator.getEstimatedMxy());
13660 assertNull(calibrator.getEstimatedMxz());
13661 assertNull(calibrator.getEstimatedMyx());
13662 assertNull(calibrator.getEstimatedMyz());
13663 assertNull(calibrator.getEstimatedMzx());
13664 assertNull(calibrator.getEstimatedMzy());
13665 assertNull(calibrator.getEstimatedCovariance());
13666 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13667 assertNotNull(calibrator.getGroundTruthGravityNorm());
13668 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13669 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13670 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13671 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13672 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13673 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13674 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13675
13676
13677 calibrator = null;
13678 try {
13679 calibrator = new KnownGravityNormAccelerometerCalibrator(
13680 -gravityNorm, measurements,
13681 bx, by, bz, sx, sy, sz, this);
13682 fail("IllegalArgumentException expected but not thrown");
13683 } catch (final IllegalArgumentException ignore) {
13684 }
13685 assertNull(calibrator);
13686 }
13687
13688 @Test
13689 public void testConstructor114() throws WrongSizeException {
13690 final Matrix ba = generateBa();
13691 final double biasX = ba.getElementAtIndex(0);
13692 final double biasY = ba.getElementAtIndex(1);
13693 final double biasZ = ba.getElementAtIndex(2);
13694
13695 final Acceleration bx = new Acceleration(biasX,
13696 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13697 final Acceleration by = new Acceleration(biasY,
13698 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13699 final Acceleration bz = new Acceleration(biasZ,
13700 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13701
13702 final Matrix ma = generateMaCommonAxis();
13703 final double sx = ma.getElementAt(0, 0);
13704 final double sy = ma.getElementAt(1, 1);
13705 final double sz = ma.getElementAt(2, 2);
13706
13707 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13708 final double latitude = Math.toRadians(
13709 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13710 final double longitude = Math.toRadians(
13711 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13712 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13713 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13714 final NEDVelocity nedVelocity = new NEDVelocity();
13715 final ECEFPosition ecefPosition = new ECEFPosition();
13716 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13717 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13718 ecefPosition, ecefVelocity);
13719 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13720 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13721 final double gravityNorm = gravity.getNorm();
13722
13723 KnownGravityNormAccelerometerCalibrator calibrator =
13724 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13725 true, bx, by, bz, sx, sy, sz);
13726
13727
13728 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13729 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13730 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13731 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13732 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13733 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13734 final Acceleration bx2 = new Acceleration(0.0,
13735 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13736 calibrator.getInitialBiasXAsAcceleration(bx2);
13737 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13738 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13739 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13740 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13741 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13742 final Acceleration by2 = new Acceleration(0.0,
13743 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13744 calibrator.getInitialBiasYAsAcceleration(by2);
13745 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13746 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13747 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13748 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13749 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13750 final Acceleration bz2 = new Acceleration(0.0,
13751 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13752 calibrator.getInitialBiasZAsAcceleration(bz2);
13753 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13754 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13755 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13756 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13757 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13758 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13759 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13760 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13761 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13762 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13763 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13764 final double[] bias1 = calibrator.getInitialBias();
13765 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13766 final double[] bias2 = new double[3];
13767 calibrator.getInitialBias(bias2);
13768 assertArrayEquals(bias1, bias2, 0.0);
13769 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13770 assertEquals(b1, ba);
13771 final Matrix b2 = new Matrix(3, 1);
13772 calibrator.getInitialBiasAsMatrix(b2);
13773 assertEquals(b1, b2);
13774 final Matrix ma1 = calibrator.getInitialMa();
13775 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13776 final Matrix ma2 = new Matrix(3, 3);
13777 calibrator.getInitialMa(ma2);
13778 assertEquals(ma1, ma2);
13779 assertNull(calibrator.getMeasurements());
13780 assertTrue(calibrator.isCommonAxisUsed());
13781 assertNull(calibrator.getListener());
13782 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13783 assertFalse(calibrator.isReady());
13784 assertFalse(calibrator.isRunning());
13785 assertNull(calibrator.getEstimatedBiases());
13786 assertFalse(calibrator.getEstimatedBiases(null));
13787 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13788 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13789 assertNull(calibrator.getEstimatedBiasFx());
13790 assertNull(calibrator.getEstimatedBiasFy());
13791 assertNull(calibrator.getEstimatedBiasFz());
13792 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13793 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13794 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13795 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13796 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13797 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13798 assertNull(calibrator.getEstimatedMa());
13799 assertNull(calibrator.getEstimatedSx());
13800 assertNull(calibrator.getEstimatedSy());
13801 assertNull(calibrator.getEstimatedSz());
13802 assertNull(calibrator.getEstimatedMxy());
13803 assertNull(calibrator.getEstimatedMxz());
13804 assertNull(calibrator.getEstimatedMyx());
13805 assertNull(calibrator.getEstimatedMyz());
13806 assertNull(calibrator.getEstimatedMzx());
13807 assertNull(calibrator.getEstimatedMzy());
13808 assertNull(calibrator.getEstimatedCovariance());
13809 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13810 assertNotNull(calibrator.getGroundTruthGravityNorm());
13811 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13812 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13813 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13814 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13815 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13816 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13817 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13818
13819
13820 calibrator = null;
13821 try {
13822 calibrator = new KnownGravityNormAccelerometerCalibrator(
13823 -gravityNorm, true, bx, by, bz,
13824 sx, sy, sz);
13825 fail("IllegalArgumentException expected but not thrown");
13826 } catch (final IllegalArgumentException ignore) {
13827 }
13828 assertNull(calibrator);
13829 }
13830
13831 @Test
13832 public void testConstructor115() throws WrongSizeException {
13833 final Matrix ba = generateBa();
13834 final double biasX = ba.getElementAtIndex(0);
13835 final double biasY = ba.getElementAtIndex(1);
13836 final double biasZ = ba.getElementAtIndex(2);
13837
13838 final Acceleration bx = new Acceleration(biasX,
13839 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13840 final Acceleration by = new Acceleration(biasY,
13841 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13842 final Acceleration bz = new Acceleration(biasZ,
13843 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13844
13845 final Matrix ma = generateMaCommonAxis();
13846 final double sx = ma.getElementAt(0, 0);
13847 final double sy = ma.getElementAt(1, 1);
13848 final double sz = ma.getElementAt(2, 2);
13849
13850 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13851 final double latitude = Math.toRadians(
13852 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13853 final double longitude = Math.toRadians(
13854 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13855 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13856 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13857 final NEDVelocity nedVelocity = new NEDVelocity();
13858 final ECEFPosition ecefPosition = new ECEFPosition();
13859 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13860 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13861 ecefPosition, ecefVelocity);
13862 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13863 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13864 final double gravityNorm = gravity.getNorm();
13865
13866 KnownGravityNormAccelerometerCalibrator calibrator =
13867 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13868 true, bx, by, bz, sx, sy, sz,
13869 this);
13870
13871
13872 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13873 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13874 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13875 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13876 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13877 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13878 final Acceleration bx2 = new Acceleration(0.0,
13879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13880 calibrator.getInitialBiasXAsAcceleration(bx2);
13881 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13882 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13883 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13884 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13885 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13886 final Acceleration by2 = new Acceleration(0.0,
13887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13888 calibrator.getInitialBiasYAsAcceleration(by2);
13889 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13890 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13891 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13892 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13893 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13894 final Acceleration bz2 = new Acceleration(0.0,
13895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13896 calibrator.getInitialBiasZAsAcceleration(bz2);
13897 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13898 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13899 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13900 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13901 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13902 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13903 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13904 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13905 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13906 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13907 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13908 final double[] bias1 = calibrator.getInitialBias();
13909 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13910 final double[] bias2 = new double[3];
13911 calibrator.getInitialBias(bias2);
13912 assertArrayEquals(bias1, bias2, 0.0);
13913 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13914 assertEquals(b1, ba);
13915 final Matrix b2 = new Matrix(3, 1);
13916 calibrator.getInitialBiasAsMatrix(b2);
13917 assertEquals(b1, b2);
13918 final Matrix ma1 = calibrator.getInitialMa();
13919 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13920 final Matrix ma2 = new Matrix(3, 3);
13921 calibrator.getInitialMa(ma2);
13922 assertEquals(ma1, ma2);
13923 assertNull(calibrator.getMeasurements());
13924 assertTrue(calibrator.isCommonAxisUsed());
13925 assertSame(calibrator.getListener(), this);
13926 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13927 assertFalse(calibrator.isReady());
13928 assertFalse(calibrator.isRunning());
13929 assertNull(calibrator.getEstimatedBiases());
13930 assertFalse(calibrator.getEstimatedBiases(null));
13931 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13932 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13933 assertNull(calibrator.getEstimatedBiasFx());
13934 assertNull(calibrator.getEstimatedBiasFy());
13935 assertNull(calibrator.getEstimatedBiasFz());
13936 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13937 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13938 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13939 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13940 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13941 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13942 assertNull(calibrator.getEstimatedMa());
13943 assertNull(calibrator.getEstimatedSx());
13944 assertNull(calibrator.getEstimatedSy());
13945 assertNull(calibrator.getEstimatedSz());
13946 assertNull(calibrator.getEstimatedMxy());
13947 assertNull(calibrator.getEstimatedMxz());
13948 assertNull(calibrator.getEstimatedMyx());
13949 assertNull(calibrator.getEstimatedMyz());
13950 assertNull(calibrator.getEstimatedMzx());
13951 assertNull(calibrator.getEstimatedMzy());
13952 assertNull(calibrator.getEstimatedCovariance());
13953 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13954 assertNotNull(calibrator.getGroundTruthGravityNorm());
13955 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13956 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13957 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13958 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13959 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13960 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13961 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13962
13963
13964 calibrator = null;
13965 try {
13966 calibrator = new KnownGravityNormAccelerometerCalibrator(
13967 -gravityNorm, true, bx, by, bz, sx, sy, sz,
13968 this);
13969 fail("IllegalArgumentException expected but not thrown");
13970 } catch (final IllegalArgumentException ignore) {
13971 }
13972 assertNull(calibrator);
13973 }
13974
13975 @Test
13976 public void testConstructor116() throws WrongSizeException {
13977 final Collection<StandardDeviationBodyKinematics> measurements =
13978 Collections.emptyList();
13979
13980 final Matrix ba = generateBa();
13981 final double biasX = ba.getElementAtIndex(0);
13982 final double biasY = ba.getElementAtIndex(1);
13983 final double biasZ = ba.getElementAtIndex(2);
13984
13985 final Acceleration bx = new Acceleration(biasX,
13986 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13987 final Acceleration by = new Acceleration(biasY,
13988 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13989 final Acceleration bz = new Acceleration(biasZ,
13990 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13991
13992 final Matrix ma = generateMaCommonAxis();
13993 final double sx = ma.getElementAt(0, 0);
13994 final double sy = ma.getElementAt(1, 1);
13995 final double sz = ma.getElementAt(2, 2);
13996
13997 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13998 final double latitude = Math.toRadians(
13999 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14000 final double longitude = Math.toRadians(
14001 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14002 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14003 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14004 final NEDVelocity nedVelocity = new NEDVelocity();
14005 final ECEFPosition ecefPosition = new ECEFPosition();
14006 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14007 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14008 ecefPosition, ecefVelocity);
14009 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14010 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14011 final double gravityNorm = gravity.getNorm();
14012
14013 KnownGravityNormAccelerometerCalibrator calibrator =
14014 new KnownGravityNormAccelerometerCalibrator(
14015 gravityNorm, measurements,
14016 true, bx, by, bz, sx, sy, sz);
14017
14018
14019 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14020 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14021 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14022 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14023 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14024 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14025 final Acceleration bx2 = new Acceleration(0.0,
14026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14027 calibrator.getInitialBiasXAsAcceleration(bx2);
14028 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14029 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14030 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14031 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14032 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14033 final Acceleration by2 = new Acceleration(0.0,
14034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14035 calibrator.getInitialBiasYAsAcceleration(by2);
14036 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14037 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14038 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14039 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14040 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14041 final Acceleration bz2 = new Acceleration(0.0,
14042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14043 calibrator.getInitialBiasZAsAcceleration(bz2);
14044 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14045 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14046 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14047 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14048 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14049 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14050 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14051 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14052 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14053 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14054 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14055 final double[] bias1 = calibrator.getInitialBias();
14056 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14057 final double[] bias2 = new double[3];
14058 calibrator.getInitialBias(bias2);
14059 assertArrayEquals(bias1, bias2, 0.0);
14060 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14061 assertEquals(b1, ba);
14062 final Matrix b2 = new Matrix(3, 1);
14063 calibrator.getInitialBiasAsMatrix(b2);
14064 assertEquals(b1, b2);
14065 final Matrix ma1 = calibrator.getInitialMa();
14066 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14067 final Matrix ma2 = new Matrix(3, 3);
14068 calibrator.getInitialMa(ma2);
14069 assertEquals(ma1, ma2);
14070 assertSame(calibrator.getMeasurements(), measurements);
14071 assertTrue(calibrator.isCommonAxisUsed());
14072 assertNull(calibrator.getListener());
14073 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14074 assertFalse(calibrator.isReady());
14075 assertFalse(calibrator.isRunning());
14076 assertNull(calibrator.getEstimatedBiases());
14077 assertFalse(calibrator.getEstimatedBiases(null));
14078 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14079 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14080 assertNull(calibrator.getEstimatedBiasFx());
14081 assertNull(calibrator.getEstimatedBiasFy());
14082 assertNull(calibrator.getEstimatedBiasFz());
14083 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14084 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14085 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14086 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14087 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14088 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14089 assertNull(calibrator.getEstimatedMa());
14090 assertNull(calibrator.getEstimatedSx());
14091 assertNull(calibrator.getEstimatedSy());
14092 assertNull(calibrator.getEstimatedSz());
14093 assertNull(calibrator.getEstimatedMxy());
14094 assertNull(calibrator.getEstimatedMxz());
14095 assertNull(calibrator.getEstimatedMyx());
14096 assertNull(calibrator.getEstimatedMyz());
14097 assertNull(calibrator.getEstimatedMzx());
14098 assertNull(calibrator.getEstimatedMzy());
14099 assertNull(calibrator.getEstimatedCovariance());
14100 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14101 assertNotNull(calibrator.getGroundTruthGravityNorm());
14102 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14103 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14104 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14105 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14106 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14107 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14108 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14109
14110
14111 calibrator = null;
14112 try {
14113 calibrator = new KnownGravityNormAccelerometerCalibrator(
14114 -gravityNorm, measurements,
14115 true, bx, by, bz, sx, sy, sz);
14116 fail("IllegalArgumentException expected but not thrown");
14117 } catch (final IllegalArgumentException ignore) {
14118 }
14119 assertNull(calibrator);
14120 }
14121
14122 @Test
14123 public void testConstructor117() throws WrongSizeException {
14124 final Collection<StandardDeviationBodyKinematics> measurements =
14125 Collections.emptyList();
14126
14127 final Matrix ba = generateBa();
14128 final double biasX = ba.getElementAtIndex(0);
14129 final double biasY = ba.getElementAtIndex(1);
14130 final double biasZ = ba.getElementAtIndex(2);
14131
14132 final Acceleration bx = new Acceleration(biasX,
14133 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14134 final Acceleration by = new Acceleration(biasY,
14135 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14136 final Acceleration bz = new Acceleration(biasZ,
14137 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14138
14139 final Matrix ma = generateMaCommonAxis();
14140 final double sx = ma.getElementAt(0, 0);
14141 final double sy = ma.getElementAt(1, 1);
14142 final double sz = ma.getElementAt(2, 2);
14143
14144 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14145 final double latitude = Math.toRadians(
14146 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14147 final double longitude = Math.toRadians(
14148 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14149 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14150 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14151 final NEDVelocity nedVelocity = new NEDVelocity();
14152 final ECEFPosition ecefPosition = new ECEFPosition();
14153 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14154 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14155 ecefPosition, ecefVelocity);
14156 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14157 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14158 final double gravityNorm = gravity.getNorm();
14159
14160 KnownGravityNormAccelerometerCalibrator calibrator =
14161 new KnownGravityNormAccelerometerCalibrator(
14162 gravityNorm, measurements,
14163 true, bx, by, bz, sx, sy, sz, this);
14164
14165
14166 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14167 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14168 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14169 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14170 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14171 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14172 final Acceleration bx2 = new Acceleration(0.0,
14173 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14174 calibrator.getInitialBiasXAsAcceleration(bx2);
14175 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14176 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14177 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14178 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14179 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14180 final Acceleration by2 = new Acceleration(0.0,
14181 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14182 calibrator.getInitialBiasYAsAcceleration(by2);
14183 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14184 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14185 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14186 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14187 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14188 final Acceleration bz2 = new Acceleration(0.0,
14189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14190 calibrator.getInitialBiasZAsAcceleration(bz2);
14191 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14192 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14193 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14194 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14195 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14196 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14197 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14198 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14199 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14200 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14201 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14202 final double[] bias1 = calibrator.getInitialBias();
14203 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14204 final double[] bias2 = new double[3];
14205 calibrator.getInitialBias(bias2);
14206 assertArrayEquals(bias1, bias2, 0.0);
14207 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14208 assertEquals(b1, ba);
14209 final Matrix b2 = new Matrix(3, 1);
14210 calibrator.getInitialBiasAsMatrix(b2);
14211 assertEquals(b1, b2);
14212 final Matrix ma1 = calibrator.getInitialMa();
14213 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14214 final Matrix ma2 = new Matrix(3, 3);
14215 calibrator.getInitialMa(ma2);
14216 assertEquals(ma1, ma2);
14217 assertSame(calibrator.getMeasurements(), measurements);
14218 assertTrue(calibrator.isCommonAxisUsed());
14219 assertSame(calibrator.getListener(), this);
14220 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14221 assertFalse(calibrator.isReady());
14222 assertFalse(calibrator.isRunning());
14223 assertNull(calibrator.getEstimatedBiases());
14224 assertFalse(calibrator.getEstimatedBiases(null));
14225 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14226 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14227 assertNull(calibrator.getEstimatedBiasFx());
14228 assertNull(calibrator.getEstimatedBiasFy());
14229 assertNull(calibrator.getEstimatedBiasFz());
14230 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14231 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14232 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14233 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14234 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14235 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14236 assertNull(calibrator.getEstimatedMa());
14237 assertNull(calibrator.getEstimatedSx());
14238 assertNull(calibrator.getEstimatedSy());
14239 assertNull(calibrator.getEstimatedSz());
14240 assertNull(calibrator.getEstimatedMxy());
14241 assertNull(calibrator.getEstimatedMxz());
14242 assertNull(calibrator.getEstimatedMyx());
14243 assertNull(calibrator.getEstimatedMyz());
14244 assertNull(calibrator.getEstimatedMzx());
14245 assertNull(calibrator.getEstimatedMzy());
14246 assertNull(calibrator.getEstimatedCovariance());
14247 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14248 assertNotNull(calibrator.getGroundTruthGravityNorm());
14249 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14250 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14251 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14252 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14253 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14254 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14255 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14256
14257
14258 calibrator = null;
14259 try {
14260 calibrator = new KnownGravityNormAccelerometerCalibrator(
14261 -gravityNorm, measurements,
14262 true, bx, by, bz, sx, sy, sz, this);
14263 fail("IllegalArgumentException expected but not thrown");
14264 } catch (final IllegalArgumentException ignore) {
14265 }
14266 assertNull(calibrator);
14267 }
14268
14269 @Test
14270 public void testConstructor118() throws WrongSizeException {
14271 final Matrix ba = generateBa();
14272 final double biasX = ba.getElementAtIndex(0);
14273 final double biasY = ba.getElementAtIndex(1);
14274 final double biasZ = ba.getElementAtIndex(2);
14275
14276 final Matrix ma = generateMaCommonAxis();
14277 final double sx = ma.getElementAt(0, 0);
14278 final double sy = ma.getElementAt(1, 1);
14279 final double sz = ma.getElementAt(2, 2);
14280 final double mxy = ma.getElementAt(0, 1);
14281 final double mxz = ma.getElementAt(0, 2);
14282 final double myx = ma.getElementAt(1, 0);
14283 final double myz = ma.getElementAt(1, 2);
14284 final double mzx = ma.getElementAt(2, 0);
14285 final double mzy = ma.getElementAt(2, 1);
14286
14287 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14288 final double latitude = Math.toRadians(
14289 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14290 final double longitude = Math.toRadians(
14291 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14292 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14293 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14294 final NEDVelocity nedVelocity = new NEDVelocity();
14295 final ECEFPosition ecefPosition = new ECEFPosition();
14296 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14297 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14298 ecefPosition, ecefVelocity);
14299 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14300 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14301 final double gravityNorm = gravity.getNorm();
14302
14303 KnownGravityNormAccelerometerCalibrator calibrator =
14304 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14305 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14306 myx, myz, mzx, mzy);
14307
14308
14309 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14310 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14311 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14312 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14313 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14314 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14315 final Acceleration bx2 = new Acceleration(0.0,
14316 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14317 calibrator.getInitialBiasXAsAcceleration(bx2);
14318 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14319 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14320 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14321 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14322 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14323 final Acceleration by2 = new Acceleration(0.0,
14324 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14325 calibrator.getInitialBiasYAsAcceleration(by2);
14326 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14327 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14328 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14329 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14330 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14331 final Acceleration bz2 = new Acceleration(0.0,
14332 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14333 calibrator.getInitialBiasZAsAcceleration(bz2);
14334 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14335 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14336 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14337 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14338 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14339 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14340 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14341 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14342 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14343 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14344 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14345 final double[] bias1 = calibrator.getInitialBias();
14346 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14347 final double[] bias2 = new double[3];
14348 calibrator.getInitialBias(bias2);
14349 assertArrayEquals(bias1, bias2, 0.0);
14350 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14351 assertEquals(b1, ba);
14352 final Matrix b2 = new Matrix(3, 1);
14353 calibrator.getInitialBiasAsMatrix(b2);
14354 assertEquals(b1, b2);
14355 final Matrix ma1 = new Matrix(3, 3);
14356 ma1.setSubmatrix(0, 0,
14357 2, 2,
14358 new double[]{sx, myx, mzx,
14359 mxy, sy, mzy,
14360 mxz, myz, sz});
14361 assertEquals(calibrator.getInitialMa(), ma1);
14362 final Matrix ma2 = new Matrix(3, 3);
14363 calibrator.getInitialMa(ma2);
14364 assertEquals(ma1, ma2);
14365 assertNull(calibrator.getMeasurements());
14366 assertFalse(calibrator.isCommonAxisUsed());
14367 assertNull(calibrator.getListener());
14368 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14369 assertFalse(calibrator.isReady());
14370 assertFalse(calibrator.isRunning());
14371 assertNull(calibrator.getEstimatedBiases());
14372 assertFalse(calibrator.getEstimatedBiases(null));
14373 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14374 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14375 assertNull(calibrator.getEstimatedBiasFx());
14376 assertNull(calibrator.getEstimatedBiasFy());
14377 assertNull(calibrator.getEstimatedBiasFz());
14378 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14379 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14380 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14381 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14382 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14383 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14384 assertNull(calibrator.getEstimatedMa());
14385 assertNull(calibrator.getEstimatedSx());
14386 assertNull(calibrator.getEstimatedSy());
14387 assertNull(calibrator.getEstimatedSz());
14388 assertNull(calibrator.getEstimatedMxy());
14389 assertNull(calibrator.getEstimatedMxz());
14390 assertNull(calibrator.getEstimatedMyx());
14391 assertNull(calibrator.getEstimatedMyz());
14392 assertNull(calibrator.getEstimatedMzx());
14393 assertNull(calibrator.getEstimatedMzy());
14394 assertNull(calibrator.getEstimatedCovariance());
14395 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14396 assertNotNull(calibrator.getGroundTruthGravityNorm());
14397 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14398 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14399 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14400 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14401 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14402 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14403 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14404
14405
14406 calibrator = null;
14407 try {
14408 calibrator = new KnownGravityNormAccelerometerCalibrator(
14409 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz,
14410 mxy, mxz, myx, myz, mzx, mzy);
14411 fail("IllegalArgumentException expected but not thrown");
14412 } catch (final IllegalArgumentException ignore) {
14413 }
14414 assertNull(calibrator);
14415 }
14416
14417 @Test
14418 public void testConstructor119() throws WrongSizeException {
14419 final Collection<StandardDeviationBodyKinematics> measurements =
14420 Collections.emptyList();
14421
14422 final Matrix ba = generateBa();
14423 final double biasX = ba.getElementAtIndex(0);
14424 final double biasY = ba.getElementAtIndex(1);
14425 final double biasZ = ba.getElementAtIndex(2);
14426
14427 final Matrix ma = generateMaCommonAxis();
14428 final double sx = ma.getElementAt(0, 0);
14429 final double sy = ma.getElementAt(1, 1);
14430 final double sz = ma.getElementAt(2, 2);
14431 final double mxy = ma.getElementAt(0, 1);
14432 final double mxz = ma.getElementAt(0, 2);
14433 final double myx = ma.getElementAt(1, 0);
14434 final double myz = ma.getElementAt(1, 2);
14435 final double mzx = ma.getElementAt(2, 0);
14436 final double mzy = ma.getElementAt(2, 1);
14437
14438 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14439 final double latitude = Math.toRadians(
14440 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14441 final double longitude = Math.toRadians(
14442 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14443 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14444 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14445 final NEDVelocity nedVelocity = new NEDVelocity();
14446 final ECEFPosition ecefPosition = new ECEFPosition();
14447 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14448 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14449 ecefPosition, ecefVelocity);
14450 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14451 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14452 final double gravityNorm = gravity.getNorm();
14453
14454 KnownGravityNormAccelerometerCalibrator calibrator =
14455 new KnownGravityNormAccelerometerCalibrator(
14456 gravityNorm, measurements,
14457 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14458 myx, myz, mzx, mzy);
14459
14460
14461 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14462 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14463 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14464 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14465 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14466 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14467 final Acceleration bx2 = new Acceleration(0.0,
14468 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14469 calibrator.getInitialBiasXAsAcceleration(bx2);
14470 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14471 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14472 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14473 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14474 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14475 final Acceleration by2 = new Acceleration(0.0,
14476 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14477 calibrator.getInitialBiasYAsAcceleration(by2);
14478 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14479 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14480 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14481 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14482 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14483 final Acceleration bz2 = new Acceleration(0.0,
14484 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14485 calibrator.getInitialBiasZAsAcceleration(bz2);
14486 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14487 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14488 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14489 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14490 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14491 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14492 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14493 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14494 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14495 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14496 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14497 final double[] bias1 = calibrator.getInitialBias();
14498 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14499 final double[] bias2 = new double[3];
14500 calibrator.getInitialBias(bias2);
14501 assertArrayEquals(bias1, bias2, 0.0);
14502 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14503 assertEquals(b1, ba);
14504 final Matrix b2 = new Matrix(3, 1);
14505 calibrator.getInitialBiasAsMatrix(b2);
14506 assertEquals(b1, b2);
14507 final Matrix ma1 = new Matrix(3, 3);
14508 ma1.setSubmatrix(0, 0,
14509 2, 2,
14510 new double[]{sx, myx, mzx,
14511 mxy, sy, mzy,
14512 mxz, myz, sz});
14513 assertEquals(calibrator.getInitialMa(), ma1);
14514 final Matrix ma2 = new Matrix(3, 3);
14515 calibrator.getInitialMa(ma2);
14516 assertEquals(ma1, ma2);
14517 assertSame(calibrator.getMeasurements(), measurements);
14518 assertFalse(calibrator.isCommonAxisUsed());
14519 assertNull(calibrator.getListener());
14520 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14521 assertFalse(calibrator.isReady());
14522 assertFalse(calibrator.isRunning());
14523 assertNull(calibrator.getEstimatedBiases());
14524 assertFalse(calibrator.getEstimatedBiases(null));
14525 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14526 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14527 assertNull(calibrator.getEstimatedBiasFx());
14528 assertNull(calibrator.getEstimatedBiasFy());
14529 assertNull(calibrator.getEstimatedBiasFz());
14530 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14531 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14532 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14533 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14534 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14535 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14536 assertNull(calibrator.getEstimatedMa());
14537 assertNull(calibrator.getEstimatedSx());
14538 assertNull(calibrator.getEstimatedSy());
14539 assertNull(calibrator.getEstimatedSz());
14540 assertNull(calibrator.getEstimatedMxy());
14541 assertNull(calibrator.getEstimatedMxz());
14542 assertNull(calibrator.getEstimatedMyx());
14543 assertNull(calibrator.getEstimatedMyz());
14544 assertNull(calibrator.getEstimatedMzx());
14545 assertNull(calibrator.getEstimatedMzy());
14546 assertNull(calibrator.getEstimatedCovariance());
14547 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14548 assertNotNull(calibrator.getGroundTruthGravityNorm());
14549 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14550 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14551 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14552 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14553 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14554 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14555 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14556
14557
14558 calibrator = null;
14559 try {
14560 calibrator = new KnownGravityNormAccelerometerCalibrator(
14561 -gravityNorm, measurements,
14562 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14563 myx, myz, mzx, mzy);
14564 fail("IllegalArgumentException expected but not thrown");
14565 } catch (final IllegalArgumentException ignore) {
14566 }
14567 assertNull(calibrator);
14568 }
14569
14570 @Test
14571 public void testConstructor120() throws WrongSizeException {
14572 final Collection<StandardDeviationBodyKinematics> measurements =
14573 Collections.emptyList();
14574
14575 final Matrix ba = generateBa();
14576 final double biasX = ba.getElementAtIndex(0);
14577 final double biasY = ba.getElementAtIndex(1);
14578 final double biasZ = ba.getElementAtIndex(2);
14579
14580 final Matrix ma = generateMaCommonAxis();
14581 final double sx = ma.getElementAt(0, 0);
14582 final double sy = ma.getElementAt(1, 1);
14583 final double sz = ma.getElementAt(2, 2);
14584 final double mxy = ma.getElementAt(0, 1);
14585 final double mxz = ma.getElementAt(0, 2);
14586 final double myx = ma.getElementAt(1, 0);
14587 final double myz = ma.getElementAt(1, 2);
14588 final double mzx = ma.getElementAt(2, 0);
14589 final double mzy = ma.getElementAt(2, 1);
14590
14591 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14592 final double latitude = Math.toRadians(
14593 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14594 final double longitude = Math.toRadians(
14595 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14596 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14597 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14598 final NEDVelocity nedVelocity = new NEDVelocity();
14599 final ECEFPosition ecefPosition = new ECEFPosition();
14600 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14601 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14602 ecefPosition, ecefVelocity);
14603 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14604 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14605 final double gravityNorm = gravity.getNorm();
14606
14607 KnownGravityNormAccelerometerCalibrator calibrator =
14608 new KnownGravityNormAccelerometerCalibrator(
14609 gravityNorm, measurements,
14610 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14611 myx, myz, mzx, mzy, this);
14612
14613
14614 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14615 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14616 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14617 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14618 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14619 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14620 final Acceleration bx2 = new Acceleration(0.0,
14621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14622 calibrator.getInitialBiasXAsAcceleration(bx2);
14623 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14624 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14625 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14626 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14627 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14628 final Acceleration by2 = new Acceleration(0.0,
14629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14630 calibrator.getInitialBiasYAsAcceleration(by2);
14631 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14632 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14633 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14634 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14635 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14636 final Acceleration bz2 = new Acceleration(0.0,
14637 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14638 calibrator.getInitialBiasZAsAcceleration(bz2);
14639 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14640 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14641 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14642 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14643 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14644 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14645 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14646 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14647 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14648 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14649 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14650 final double[] bias1 = calibrator.getInitialBias();
14651 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14652 final double[] bias2 = new double[3];
14653 calibrator.getInitialBias(bias2);
14654 assertArrayEquals(bias1, bias2, 0.0);
14655 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14656 assertEquals(b1, ba);
14657 final Matrix b2 = new Matrix(3, 1);
14658 calibrator.getInitialBiasAsMatrix(b2);
14659 assertEquals(b1, b2);
14660 final Matrix ma1 = new Matrix(3, 3);
14661 ma1.setSubmatrix(0, 0,
14662 2, 2,
14663 new double[]{sx, myx, mzx,
14664 mxy, sy, mzy,
14665 mxz, myz, sz});
14666 assertEquals(calibrator.getInitialMa(), ma1);
14667 final Matrix ma2 = new Matrix(3, 3);
14668 calibrator.getInitialMa(ma2);
14669 assertEquals(ma1, ma2);
14670 assertSame(calibrator.getMeasurements(), measurements);
14671 assertFalse(calibrator.isCommonAxisUsed());
14672 assertSame(calibrator.getListener(), this);
14673 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14674 assertFalse(calibrator.isReady());
14675 assertFalse(calibrator.isRunning());
14676 assertNull(calibrator.getEstimatedBiases());
14677 assertFalse(calibrator.getEstimatedBiases(null));
14678 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14679 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14680 assertNull(calibrator.getEstimatedBiasFx());
14681 assertNull(calibrator.getEstimatedBiasFy());
14682 assertNull(calibrator.getEstimatedBiasFz());
14683 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14684 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14685 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14686 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14687 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14688 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14689 assertNull(calibrator.getEstimatedMa());
14690 assertNull(calibrator.getEstimatedSx());
14691 assertNull(calibrator.getEstimatedSy());
14692 assertNull(calibrator.getEstimatedSz());
14693 assertNull(calibrator.getEstimatedMxy());
14694 assertNull(calibrator.getEstimatedMxz());
14695 assertNull(calibrator.getEstimatedMyx());
14696 assertNull(calibrator.getEstimatedMyz());
14697 assertNull(calibrator.getEstimatedMzx());
14698 assertNull(calibrator.getEstimatedMzy());
14699 assertNull(calibrator.getEstimatedCovariance());
14700 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14701 assertNotNull(calibrator.getGroundTruthGravityNorm());
14702 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14703 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14704 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14705 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14706 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14707 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14708 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14709
14710
14711 calibrator = null;
14712 try {
14713 calibrator = new KnownGravityNormAccelerometerCalibrator(
14714 -gravityNorm, measurements,
14715 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14716 myx, myz, mzx, mzy, this);
14717 fail("IllegalArgumentException expected but not thrown");
14718 } catch (final IllegalArgumentException ignore) {
14719 }
14720 assertNull(calibrator);
14721 }
14722
14723 @Test
14724 public void testConstructor121() throws WrongSizeException {
14725 final Matrix ba = generateBa();
14726 final double biasX = ba.getElementAtIndex(0);
14727 final double biasY = ba.getElementAtIndex(1);
14728 final double biasZ = ba.getElementAtIndex(2);
14729
14730 final Matrix ma = generateMaCommonAxis();
14731 final double sx = ma.getElementAt(0, 0);
14732 final double sy = ma.getElementAt(1, 1);
14733 final double sz = ma.getElementAt(2, 2);
14734 final double mxy = ma.getElementAt(0, 1);
14735 final double mxz = ma.getElementAt(0, 2);
14736 final double myx = ma.getElementAt(1, 0);
14737 final double myz = ma.getElementAt(1, 2);
14738 final double mzx = ma.getElementAt(2, 0);
14739 final double mzy = ma.getElementAt(2, 1);
14740
14741 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14742 final double latitude = Math.toRadians(
14743 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14744 final double longitude = Math.toRadians(
14745 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14746 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14747 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14748 final NEDVelocity nedVelocity = new NEDVelocity();
14749 final ECEFPosition ecefPosition = new ECEFPosition();
14750 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14751 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14752 ecefPosition, ecefVelocity);
14753 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14754 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14755 final double gravityNorm = gravity.getNorm();
14756
14757 KnownGravityNormAccelerometerCalibrator calibrator =
14758 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14759 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14760 myx, myz, mzx, mzy);
14761
14762
14763 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14764 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14765 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14766 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14767 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14768 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14769 final Acceleration bx2 = new Acceleration(0.0,
14770 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14771 calibrator.getInitialBiasXAsAcceleration(bx2);
14772 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14773 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14774 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14775 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14776 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14777 final Acceleration by2 = new Acceleration(0.0,
14778 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14779 calibrator.getInitialBiasYAsAcceleration(by2);
14780 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14781 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14782 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14783 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14784 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14785 final Acceleration bz2 = new Acceleration(0.0,
14786 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14787 calibrator.getInitialBiasZAsAcceleration(bz2);
14788 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14789 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14790 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14791 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14792 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14793 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14794 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14795 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14796 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14797 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14798 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14799 final double[] bias1 = calibrator.getInitialBias();
14800 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14801 final double[] bias2 = new double[3];
14802 calibrator.getInitialBias(bias2);
14803 assertArrayEquals(bias1, bias2, 0.0);
14804 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14805 assertEquals(b1, ba);
14806 final Matrix b2 = new Matrix(3, 1);
14807 calibrator.getInitialBiasAsMatrix(b2);
14808 assertEquals(b1, b2);
14809 final Matrix ma1 = new Matrix(3, 3);
14810 ma1.setSubmatrix(0, 0,
14811 2, 2,
14812 new double[]{sx, myx, mzx,
14813 mxy, sy, mzy,
14814 mxz, myz, sz});
14815 assertEquals(calibrator.getInitialMa(), ma1);
14816 final Matrix ma2 = new Matrix(3, 3);
14817 calibrator.getInitialMa(ma2);
14818 assertEquals(ma1, ma2);
14819 assertNull(calibrator.getMeasurements());
14820 assertTrue(calibrator.isCommonAxisUsed());
14821 assertNull(calibrator.getListener());
14822 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14823 assertFalse(calibrator.isReady());
14824 assertFalse(calibrator.isRunning());
14825 assertNull(calibrator.getEstimatedBiases());
14826 assertFalse(calibrator.getEstimatedBiases(null));
14827 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14828 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14829 assertNull(calibrator.getEstimatedBiasFx());
14830 assertNull(calibrator.getEstimatedBiasFy());
14831 assertNull(calibrator.getEstimatedBiasFz());
14832 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14833 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14834 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14835 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14836 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14837 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14838 assertNull(calibrator.getEstimatedMa());
14839 assertNull(calibrator.getEstimatedSx());
14840 assertNull(calibrator.getEstimatedSy());
14841 assertNull(calibrator.getEstimatedSz());
14842 assertNull(calibrator.getEstimatedMxy());
14843 assertNull(calibrator.getEstimatedMxz());
14844 assertNull(calibrator.getEstimatedMyx());
14845 assertNull(calibrator.getEstimatedMyz());
14846 assertNull(calibrator.getEstimatedMzx());
14847 assertNull(calibrator.getEstimatedMzy());
14848 assertNull(calibrator.getEstimatedCovariance());
14849 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14850 assertNotNull(calibrator.getGroundTruthGravityNorm());
14851 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14852 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14853 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14854 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14855 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14856 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14857 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14858
14859
14860 calibrator = null;
14861 try {
14862 calibrator = new KnownGravityNormAccelerometerCalibrator(
14863 -gravityNorm, true, biasX, biasY, biasZ,
14864 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14865 fail("IllegalArgumentException expected but not thrown");
14866 } catch (final IllegalArgumentException ignore) {
14867 }
14868 assertNull(calibrator);
14869 }
14870
14871 @Test
14872 public void testConstructor122() throws WrongSizeException {
14873 final Matrix ba = generateBa();
14874 final double biasX = ba.getElementAtIndex(0);
14875 final double biasY = ba.getElementAtIndex(1);
14876 final double biasZ = ba.getElementAtIndex(2);
14877
14878 final Matrix ma = generateMaCommonAxis();
14879 final double sx = ma.getElementAt(0, 0);
14880 final double sy = ma.getElementAt(1, 1);
14881 final double sz = ma.getElementAt(2, 2);
14882 final double mxy = ma.getElementAt(0, 1);
14883 final double mxz = ma.getElementAt(0, 2);
14884 final double myx = ma.getElementAt(1, 0);
14885 final double myz = ma.getElementAt(1, 2);
14886 final double mzx = ma.getElementAt(2, 0);
14887 final double mzy = ma.getElementAt(2, 1);
14888
14889 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14890 final double latitude = Math.toRadians(
14891 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14892 final double longitude = Math.toRadians(
14893 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14894 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14895 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14896 final NEDVelocity nedVelocity = new NEDVelocity();
14897 final ECEFPosition ecefPosition = new ECEFPosition();
14898 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14899 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14900 ecefPosition, ecefVelocity);
14901 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14902 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14903 final double gravityNorm = gravity.getNorm();
14904
14905 KnownGravityNormAccelerometerCalibrator calibrator =
14906 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14907 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14908 myx, myz, mzx, mzy, this);
14909
14910
14911 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14912 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14913 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14914 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14915 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14916 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14917 final Acceleration bx2 = new Acceleration(0.0,
14918 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14919 calibrator.getInitialBiasXAsAcceleration(bx2);
14920 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14921 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14922 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14923 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14924 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14925 final Acceleration by2 = new Acceleration(0.0,
14926 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14927 calibrator.getInitialBiasYAsAcceleration(by2);
14928 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14929 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14930 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14931 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14932 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14933 final Acceleration bz2 = new Acceleration(0.0,
14934 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14935 calibrator.getInitialBiasZAsAcceleration(bz2);
14936 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14937 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14938 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14939 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14940 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14941 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14942 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14943 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14944 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14945 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14946 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14947 final double[] bias1 = calibrator.getInitialBias();
14948 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14949 final double[] bias2 = new double[3];
14950 calibrator.getInitialBias(bias2);
14951 assertArrayEquals(bias1, bias2, 0.0);
14952 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14953 assertEquals(b1, ba);
14954 final Matrix b2 = new Matrix(3, 1);
14955 calibrator.getInitialBiasAsMatrix(b2);
14956 assertEquals(b1, b2);
14957 final Matrix ma1 = new Matrix(3, 3);
14958 ma1.setSubmatrix(0, 0,
14959 2, 2,
14960 new double[]{sx, myx, mzx,
14961 mxy, sy, mzy,
14962 mxz, myz, sz});
14963 assertEquals(calibrator.getInitialMa(), ma1);
14964 final Matrix ma2 = new Matrix(3, 3);
14965 calibrator.getInitialMa(ma2);
14966 assertEquals(ma1, ma2);
14967 assertNull(calibrator.getMeasurements());
14968 assertTrue(calibrator.isCommonAxisUsed());
14969 assertSame(calibrator.getListener(), this);
14970 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14971 assertFalse(calibrator.isReady());
14972 assertFalse(calibrator.isRunning());
14973 assertNull(calibrator.getEstimatedBiases());
14974 assertFalse(calibrator.getEstimatedBiases(null));
14975 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14976 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14977 assertNull(calibrator.getEstimatedBiasFx());
14978 assertNull(calibrator.getEstimatedBiasFy());
14979 assertNull(calibrator.getEstimatedBiasFz());
14980 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14981 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14982 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14983 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14984 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14985 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14986 assertNull(calibrator.getEstimatedMa());
14987 assertNull(calibrator.getEstimatedSx());
14988 assertNull(calibrator.getEstimatedSy());
14989 assertNull(calibrator.getEstimatedSz());
14990 assertNull(calibrator.getEstimatedMxy());
14991 assertNull(calibrator.getEstimatedMxz());
14992 assertNull(calibrator.getEstimatedMyx());
14993 assertNull(calibrator.getEstimatedMyz());
14994 assertNull(calibrator.getEstimatedMzx());
14995 assertNull(calibrator.getEstimatedMzy());
14996 assertNull(calibrator.getEstimatedCovariance());
14997 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14998 assertNotNull(calibrator.getGroundTruthGravityNorm());
14999 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15000 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15001 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15002 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15003 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15004 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15005 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15006
15007
15008 calibrator = null;
15009 try {
15010 calibrator = new KnownGravityNormAccelerometerCalibrator(
15011 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
15012 mxy, mxz, myx, myz, mzx, mzy, this);
15013 fail("IllegalArgumentException expected but not thrown");
15014 } catch (final IllegalArgumentException ignore) {
15015 }
15016 assertNull(calibrator);
15017 }
15018
15019 @Test
15020 public void testConstructor123() throws WrongSizeException {
15021 final Collection<StandardDeviationBodyKinematics> measurements =
15022 Collections.emptyList();
15023
15024 final Matrix ba = generateBa();
15025 final double biasX = ba.getElementAtIndex(0);
15026 final double biasY = ba.getElementAtIndex(1);
15027 final double biasZ = ba.getElementAtIndex(2);
15028
15029 final Matrix ma = generateMaCommonAxis();
15030 final double sx = ma.getElementAt(0, 0);
15031 final double sy = ma.getElementAt(1, 1);
15032 final double sz = ma.getElementAt(2, 2);
15033 final double mxy = ma.getElementAt(0, 1);
15034 final double mxz = ma.getElementAt(0, 2);
15035 final double myx = ma.getElementAt(1, 0);
15036 final double myz = ma.getElementAt(1, 2);
15037 final double mzx = ma.getElementAt(2, 0);
15038 final double mzy = ma.getElementAt(2, 1);
15039
15040 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15041 final double latitude = Math.toRadians(
15042 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15043 final double longitude = Math.toRadians(
15044 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15045 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15046 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15047 final NEDVelocity nedVelocity = new NEDVelocity();
15048 final ECEFPosition ecefPosition = new ECEFPosition();
15049 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15050 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15051 ecefPosition, ecefVelocity);
15052 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15053 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15054 final double gravityNorm = gravity.getNorm();
15055
15056 KnownGravityNormAccelerometerCalibrator calibrator =
15057 new KnownGravityNormAccelerometerCalibrator(
15058 gravityNorm, measurements,
15059 true, biasX, biasY, biasZ,
15060 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
15061
15062
15063 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15064 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15065 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15066 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15067 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15068 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15069 final Acceleration bx2 = new Acceleration(0.0,
15070 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15071 calibrator.getInitialBiasXAsAcceleration(bx2);
15072 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15073 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15074 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15075 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15076 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15077 final Acceleration by2 = new Acceleration(0.0,
15078 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15079 calibrator.getInitialBiasYAsAcceleration(by2);
15080 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15081 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15082 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15083 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15084 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15085 final Acceleration bz2 = new Acceleration(0.0,
15086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15087 calibrator.getInitialBiasZAsAcceleration(bz2);
15088 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15089 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15090 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15091 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15092 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15093 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15094 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15095 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15096 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15097 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15098 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15099 final double[] bias1 = calibrator.getInitialBias();
15100 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15101 final double[] bias2 = new double[3];
15102 calibrator.getInitialBias(bias2);
15103 assertArrayEquals(bias1, bias2, 0.0);
15104 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15105 assertEquals(b1, ba);
15106 final Matrix b2 = new Matrix(3, 1);
15107 calibrator.getInitialBiasAsMatrix(b2);
15108 assertEquals(b1, b2);
15109 final Matrix ma1 = new Matrix(3, 3);
15110 ma1.setSubmatrix(0, 0,
15111 2, 2,
15112 new double[]{sx, myx, mzx,
15113 mxy, sy, mzy,
15114 mxz, myz, sz});
15115 assertEquals(calibrator.getInitialMa(), ma1);
15116 final Matrix ma2 = new Matrix(3, 3);
15117 calibrator.getInitialMa(ma2);
15118 assertEquals(ma1, ma2);
15119 assertSame(calibrator.getMeasurements(), measurements);
15120 assertTrue(calibrator.isCommonAxisUsed());
15121 assertNull(calibrator.getListener());
15122 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15123 assertFalse(calibrator.isReady());
15124 assertFalse(calibrator.isRunning());
15125 assertNull(calibrator.getEstimatedBiases());
15126 assertFalse(calibrator.getEstimatedBiases(null));
15127 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15128 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15129 assertNull(calibrator.getEstimatedBiasFx());
15130 assertNull(calibrator.getEstimatedBiasFy());
15131 assertNull(calibrator.getEstimatedBiasFz());
15132 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15133 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15134 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15135 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15136 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15137 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15138 assertNull(calibrator.getEstimatedMa());
15139 assertNull(calibrator.getEstimatedSx());
15140 assertNull(calibrator.getEstimatedSy());
15141 assertNull(calibrator.getEstimatedSz());
15142 assertNull(calibrator.getEstimatedMxy());
15143 assertNull(calibrator.getEstimatedMxz());
15144 assertNull(calibrator.getEstimatedMyx());
15145 assertNull(calibrator.getEstimatedMyz());
15146 assertNull(calibrator.getEstimatedMzx());
15147 assertNull(calibrator.getEstimatedMzy());
15148 assertNull(calibrator.getEstimatedCovariance());
15149 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15150 assertNotNull(calibrator.getGroundTruthGravityNorm());
15151 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15152 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15153 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15154 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15155 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15156 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15157 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15158
15159
15160 calibrator = null;
15161 try {
15162 calibrator = new KnownGravityNormAccelerometerCalibrator(
15163 -gravityNorm, measurements,
15164 true, biasX, biasY, biasZ,
15165 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
15166 fail("IllegalArgumentException expected but not thrown");
15167 } catch (final IllegalArgumentException ignore) {
15168 }
15169 assertNull(calibrator);
15170 }
15171
15172 @Test
15173 public void testConstructor124() throws WrongSizeException {
15174 final Collection<StandardDeviationBodyKinematics> measurements =
15175 Collections.emptyList();
15176
15177 final Matrix ba = generateBa();
15178 final double biasX = ba.getElementAtIndex(0);
15179 final double biasY = ba.getElementAtIndex(1);
15180 final double biasZ = ba.getElementAtIndex(2);
15181
15182 final Matrix ma = generateMaCommonAxis();
15183 final double sx = ma.getElementAt(0, 0);
15184 final double sy = ma.getElementAt(1, 1);
15185 final double sz = ma.getElementAt(2, 2);
15186 final double mxy = ma.getElementAt(0, 1);
15187 final double mxz = ma.getElementAt(0, 2);
15188 final double myx = ma.getElementAt(1, 0);
15189 final double myz = ma.getElementAt(1, 2);
15190 final double mzx = ma.getElementAt(2, 0);
15191 final double mzy = ma.getElementAt(2, 1);
15192
15193 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15194 final double latitude = Math.toRadians(
15195 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15196 final double longitude = Math.toRadians(
15197 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15198 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15199 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15200 final NEDVelocity nedVelocity = new NEDVelocity();
15201 final ECEFPosition ecefPosition = new ECEFPosition();
15202 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15203 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15204 ecefPosition, ecefVelocity);
15205 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15206 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15207 final double gravityNorm = gravity.getNorm();
15208
15209 KnownGravityNormAccelerometerCalibrator calibrator =
15210 new KnownGravityNormAccelerometerCalibrator(
15211 gravityNorm, measurements,
15212 true, biasX, biasY, biasZ,
15213 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
15214 this);
15215
15216
15217 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15218 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15219 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15220 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15221 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15222 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15223 final Acceleration bx2 = new Acceleration(0.0,
15224 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15225 calibrator.getInitialBiasXAsAcceleration(bx2);
15226 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15227 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15228 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15229 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15230 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15231 final Acceleration by2 = new Acceleration(0.0,
15232 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15233 calibrator.getInitialBiasYAsAcceleration(by2);
15234 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15235 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15236 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15237 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15238 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15239 final Acceleration bz2 = new Acceleration(0.0,
15240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15241 calibrator.getInitialBiasZAsAcceleration(bz2);
15242 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15243 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15244 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15245 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15246 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15247 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15248 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15249 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15250 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15251 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15252 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15253 final double[] bias1 = calibrator.getInitialBias();
15254 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15255 final double[] bias2 = new double[3];
15256 calibrator.getInitialBias(bias2);
15257 assertArrayEquals(bias1, bias2, 0.0);
15258 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15259 assertEquals(b1, ba);
15260 final Matrix b2 = new Matrix(3, 1);
15261 calibrator.getInitialBiasAsMatrix(b2);
15262 assertEquals(b1, b2);
15263 final Matrix ma1 = new Matrix(3, 3);
15264 ma1.setSubmatrix(0, 0,
15265 2, 2,
15266 new double[]{sx, myx, mzx,
15267 mxy, sy, mzy,
15268 mxz, myz, sz});
15269 assertEquals(calibrator.getInitialMa(), ma1);
15270 final Matrix ma2 = new Matrix(3, 3);
15271 calibrator.getInitialMa(ma2);
15272 assertEquals(ma1, ma2);
15273 assertSame(calibrator.getMeasurements(), measurements);
15274 assertTrue(calibrator.isCommonAxisUsed());
15275 assertSame(calibrator.getListener(), this);
15276 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15277 assertFalse(calibrator.isReady());
15278 assertFalse(calibrator.isRunning());
15279 assertNull(calibrator.getEstimatedBiases());
15280 assertFalse(calibrator.getEstimatedBiases(null));
15281 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15282 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15283 assertNull(calibrator.getEstimatedBiasFx());
15284 assertNull(calibrator.getEstimatedBiasFy());
15285 assertNull(calibrator.getEstimatedBiasFz());
15286 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15287 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15288 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15289 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15290 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15291 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15292 assertNull(calibrator.getEstimatedMa());
15293 assertNull(calibrator.getEstimatedSx());
15294 assertNull(calibrator.getEstimatedSy());
15295 assertNull(calibrator.getEstimatedSz());
15296 assertNull(calibrator.getEstimatedMxy());
15297 assertNull(calibrator.getEstimatedMxz());
15298 assertNull(calibrator.getEstimatedMyx());
15299 assertNull(calibrator.getEstimatedMyz());
15300 assertNull(calibrator.getEstimatedMzx());
15301 assertNull(calibrator.getEstimatedMzy());
15302 assertNull(calibrator.getEstimatedCovariance());
15303 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15304 assertNotNull(calibrator.getGroundTruthGravityNorm());
15305 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15306 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15307 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15308 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15309 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15310 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15311 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15312
15313
15314 calibrator = null;
15315 try {
15316 calibrator = new KnownGravityNormAccelerometerCalibrator(
15317 -gravityNorm, measurements,
15318 true, biasX, biasY, biasZ,
15319 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
15320 this);
15321 fail("IllegalArgumentException expected but not thrown");
15322 } catch (final IllegalArgumentException ignore) {
15323 }
15324 assertNull(calibrator);
15325 }
15326
15327 @Test
15328 public void testConstructor125() throws WrongSizeException {
15329 final Matrix ba = generateBa();
15330 final double biasX = ba.getElementAtIndex(0);
15331 final double biasY = ba.getElementAtIndex(1);
15332 final double biasZ = ba.getElementAtIndex(2);
15333
15334 final Acceleration bx = new Acceleration(biasX,
15335 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15336 final Acceleration by = new Acceleration(biasY,
15337 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15338 final Acceleration bz = new Acceleration(biasZ,
15339 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15340
15341 final Matrix ma = generateMaCommonAxis();
15342 final double sx = ma.getElementAt(0, 0);
15343 final double sy = ma.getElementAt(1, 1);
15344 final double sz = ma.getElementAt(2, 2);
15345 final double mxy = ma.getElementAt(0, 1);
15346 final double mxz = ma.getElementAt(0, 2);
15347 final double myx = ma.getElementAt(1, 0);
15348 final double myz = ma.getElementAt(1, 2);
15349 final double mzx = ma.getElementAt(2, 0);
15350 final double mzy = ma.getElementAt(2, 1);
15351
15352 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15353 final double latitude = Math.toRadians(
15354 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15355 final double longitude = Math.toRadians(
15356 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15357 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15358 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15359 final NEDVelocity nedVelocity = new NEDVelocity();
15360 final ECEFPosition ecefPosition = new ECEFPosition();
15361 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15362 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15363 ecefPosition, ecefVelocity);
15364 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15365 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15366 final double gravityNorm = gravity.getNorm();
15367
15368 KnownGravityNormAccelerometerCalibrator calibrator =
15369 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
15370 bx, by, bz, sx, sy, sz, mxy, mxz,
15371 myx, myz, mzx, mzy);
15372
15373
15374 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15375 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15376 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15377 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15378 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15379 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15380 final Acceleration bx2 = new Acceleration(0.0,
15381 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15382 calibrator.getInitialBiasXAsAcceleration(bx2);
15383 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15384 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15385 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15386 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15387 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15388 final Acceleration by2 = new Acceleration(0.0,
15389 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15390 calibrator.getInitialBiasYAsAcceleration(by2);
15391 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15392 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15393 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15394 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15395 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15396 final Acceleration bz2 = new Acceleration(0.0,
15397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15398 calibrator.getInitialBiasZAsAcceleration(bz2);
15399 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15400 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15401 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15402 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15403 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15404 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15405 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15406 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15407 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15408 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15409 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15410 final double[] bias1 = calibrator.getInitialBias();
15411 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15412 final double[] bias2 = new double[3];
15413 calibrator.getInitialBias(bias2);
15414 assertArrayEquals(bias1, bias2, 0.0);
15415 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15416 assertEquals(b1, ba);
15417 final Matrix b2 = new Matrix(3, 1);
15418 calibrator.getInitialBiasAsMatrix(b2);
15419 assertEquals(b1, b2);
15420 final Matrix ma1 = new Matrix(3, 3);
15421 ma1.setSubmatrix(0, 0,
15422 2, 2,
15423 new double[]{sx, myx, mzx,
15424 mxy, sy, mzy,
15425 mxz, myz, sz});
15426 assertEquals(calibrator.getInitialMa(), ma1);
15427 final Matrix ma2 = new Matrix(3, 3);
15428 calibrator.getInitialMa(ma2);
15429 assertEquals(ma1, ma2);
15430 assertNull(calibrator.getMeasurements());
15431 assertFalse(calibrator.isCommonAxisUsed());
15432 assertNull(calibrator.getListener());
15433 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15434 assertFalse(calibrator.isReady());
15435 assertFalse(calibrator.isRunning());
15436 assertNull(calibrator.getEstimatedBiases());
15437 assertFalse(calibrator.getEstimatedBiases(null));
15438 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15439 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15440 assertNull(calibrator.getEstimatedBiasFx());
15441 assertNull(calibrator.getEstimatedBiasFy());
15442 assertNull(calibrator.getEstimatedBiasFz());
15443 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15444 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15445 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15446 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15447 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15448 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15449 assertNull(calibrator.getEstimatedMa());
15450 assertNull(calibrator.getEstimatedSx());
15451 assertNull(calibrator.getEstimatedSy());
15452 assertNull(calibrator.getEstimatedSz());
15453 assertNull(calibrator.getEstimatedMxy());
15454 assertNull(calibrator.getEstimatedMxz());
15455 assertNull(calibrator.getEstimatedMyx());
15456 assertNull(calibrator.getEstimatedMyz());
15457 assertNull(calibrator.getEstimatedMzx());
15458 assertNull(calibrator.getEstimatedMzy());
15459 assertNull(calibrator.getEstimatedCovariance());
15460 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15461 assertNotNull(calibrator.getGroundTruthGravityNorm());
15462 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15463 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15464 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15465 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15466 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15467 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15468 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15469
15470
15471 calibrator = null;
15472 try {
15473 calibrator = new KnownGravityNormAccelerometerCalibrator(
15474 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
15475 myx, myz, mzx, mzy);
15476 fail("IllegalArgumentException expected but not thrown");
15477 } catch (final IllegalArgumentException ignore) {
15478 }
15479 assertNull(calibrator);
15480 }
15481
15482 @Test
15483 public void testConstructor126() throws WrongSizeException {
15484 final Matrix ba = generateBa();
15485 final double biasX = ba.getElementAtIndex(0);
15486 final double biasY = ba.getElementAtIndex(1);
15487 final double biasZ = ba.getElementAtIndex(2);
15488
15489 final Acceleration bx = new Acceleration(biasX,
15490 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15491 final Acceleration by = new Acceleration(biasY,
15492 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15493 final Acceleration bz = new Acceleration(biasZ,
15494 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15495
15496 final Matrix ma = generateMaCommonAxis();
15497 final double sx = ma.getElementAt(0, 0);
15498 final double sy = ma.getElementAt(1, 1);
15499 final double sz = ma.getElementAt(2, 2);
15500 final double mxy = ma.getElementAt(0, 1);
15501 final double mxz = ma.getElementAt(0, 2);
15502 final double myx = ma.getElementAt(1, 0);
15503 final double myz = ma.getElementAt(1, 2);
15504 final double mzx = ma.getElementAt(2, 0);
15505 final double mzy = ma.getElementAt(2, 1);
15506
15507 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15508 final double latitude = Math.toRadians(
15509 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15510 final double longitude = Math.toRadians(
15511 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15512 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15513 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15514 final NEDVelocity nedVelocity = new NEDVelocity();
15515 final ECEFPosition ecefPosition = new ECEFPosition();
15516 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15517 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15518 ecefPosition, ecefVelocity);
15519 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15520 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15521 final double gravityNorm = gravity.getNorm();
15522
15523 KnownGravityNormAccelerometerCalibrator calibrator =
15524 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
15525 bx, by, bz, sx, sy, sz, mxy, mxz,
15526 myx, myz, mzx, mzy, this);
15527
15528
15529 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15530 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15531 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15532 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15533 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15534 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15535 final Acceleration bx2 = new Acceleration(0.0,
15536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15537 calibrator.getInitialBiasXAsAcceleration(bx2);
15538 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15539 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15540 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15541 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15542 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15543 final Acceleration by2 = new Acceleration(0.0,
15544 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15545 calibrator.getInitialBiasYAsAcceleration(by2);
15546 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15547 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15548 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15549 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15550 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15551 final Acceleration bz2 = new Acceleration(0.0,
15552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15553 calibrator.getInitialBiasZAsAcceleration(bz2);
15554 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15555 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15556 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15557 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15558 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15559 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15560 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15561 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15562 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15563 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15564 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15565 final double[] bias1 = calibrator.getInitialBias();
15566 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15567 final double[] bias2 = new double[3];
15568 calibrator.getInitialBias(bias2);
15569 assertArrayEquals(bias1, bias2, 0.0);
15570 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15571 assertEquals(b1, ba);
15572 final Matrix b2 = new Matrix(3, 1);
15573 calibrator.getInitialBiasAsMatrix(b2);
15574 assertEquals(b1, b2);
15575 final Matrix ma1 = new Matrix(3, 3);
15576 ma1.setSubmatrix(0, 0,
15577 2, 2,
15578 new double[]{sx, myx, mzx,
15579 mxy, sy, mzy,
15580 mxz, myz, sz});
15581 assertEquals(calibrator.getInitialMa(), ma1);
15582 final Matrix ma2 = new Matrix(3, 3);
15583 calibrator.getInitialMa(ma2);
15584 assertEquals(ma1, ma2);
15585 assertNull(calibrator.getMeasurements());
15586 assertFalse(calibrator.isCommonAxisUsed());
15587 assertSame(calibrator.getListener(), this);
15588 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15589 assertFalse(calibrator.isReady());
15590 assertFalse(calibrator.isRunning());
15591 assertNull(calibrator.getEstimatedBiases());
15592 assertFalse(calibrator.getEstimatedBiases(null));
15593 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15594 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15595 assertNull(calibrator.getEstimatedBiasFx());
15596 assertNull(calibrator.getEstimatedBiasFy());
15597 assertNull(calibrator.getEstimatedBiasFz());
15598 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15599 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15600 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15601 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15602 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15603 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15604 assertNull(calibrator.getEstimatedMa());
15605 assertNull(calibrator.getEstimatedSx());
15606 assertNull(calibrator.getEstimatedSy());
15607 assertNull(calibrator.getEstimatedSz());
15608 assertNull(calibrator.getEstimatedMxy());
15609 assertNull(calibrator.getEstimatedMxz());
15610 assertNull(calibrator.getEstimatedMyx());
15611 assertNull(calibrator.getEstimatedMyz());
15612 assertNull(calibrator.getEstimatedMzx());
15613 assertNull(calibrator.getEstimatedMzy());
15614 assertNull(calibrator.getEstimatedCovariance());
15615 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15616 assertNotNull(calibrator.getGroundTruthGravityNorm());
15617 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15618 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15619 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15620 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15621 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15622 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15623 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15624
15625
15626 calibrator = null;
15627 try {
15628 calibrator = new KnownGravityNormAccelerometerCalibrator(
15629 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
15630 myx, myz, mzx, mzy, this);
15631 fail("IllegalArgumentException expected but not thrown");
15632 } catch (final IllegalArgumentException ignore) {
15633 }
15634 assertNull(calibrator);
15635 }
15636
15637 @Test
15638 public void testConstructor127() throws WrongSizeException {
15639 final Collection<StandardDeviationBodyKinematics> measurements =
15640 Collections.emptyList();
15641
15642 final Matrix ba = generateBa();
15643 final double biasX = ba.getElementAtIndex(0);
15644 final double biasY = ba.getElementAtIndex(1);
15645 final double biasZ = ba.getElementAtIndex(2);
15646
15647 final Acceleration bx = new Acceleration(biasX,
15648 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15649 final Acceleration by = new Acceleration(biasY,
15650 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15651 final Acceleration bz = new Acceleration(biasZ,
15652 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15653
15654 final Matrix ma = generateMaCommonAxis();
15655 final double sx = ma.getElementAt(0, 0);
15656 final double sy = ma.getElementAt(1, 1);
15657 final double sz = ma.getElementAt(2, 2);
15658 final double mxy = ma.getElementAt(0, 1);
15659 final double mxz = ma.getElementAt(0, 2);
15660 final double myx = ma.getElementAt(1, 0);
15661 final double myz = ma.getElementAt(1, 2);
15662 final double mzx = ma.getElementAt(2, 0);
15663 final double mzy = ma.getElementAt(2, 1);
15664
15665 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15666 final double latitude = Math.toRadians(
15667 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15668 final double longitude = Math.toRadians(
15669 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15670 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15671 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15672 final NEDVelocity nedVelocity = new NEDVelocity();
15673 final ECEFPosition ecefPosition = new ECEFPosition();
15674 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15675 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15676 ecefPosition, ecefVelocity);
15677 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15678 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15679 final double gravityNorm = gravity.getNorm();
15680
15681 KnownGravityNormAccelerometerCalibrator calibrator =
15682 new KnownGravityNormAccelerometerCalibrator(
15683 gravityNorm, measurements,
15684 bx, by, bz, sx, sy, sz, mxy, mxz,
15685 myx, myz, mzx, mzy);
15686
15687
15688 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15689 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15690 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15691 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15692 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15693 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15694 final Acceleration bx2 = new Acceleration(0.0,
15695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15696 calibrator.getInitialBiasXAsAcceleration(bx2);
15697 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15698 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15699 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15700 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15701 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15702 final Acceleration by2 = new Acceleration(0.0,
15703 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15704 calibrator.getInitialBiasYAsAcceleration(by2);
15705 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15706 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15707 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15708 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15709 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15710 final Acceleration bz2 = new Acceleration(0.0,
15711 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15712 calibrator.getInitialBiasZAsAcceleration(bz2);
15713 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15714 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15715 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15716 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15717 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15718 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15719 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15720 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15721 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15722 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15723 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15724 final double[] bias1 = calibrator.getInitialBias();
15725 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15726 final double[] bias2 = new double[3];
15727 calibrator.getInitialBias(bias2);
15728 assertArrayEquals(bias1, bias2, 0.0);
15729 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15730 assertEquals(b1, ba);
15731 final Matrix b2 = new Matrix(3, 1);
15732 calibrator.getInitialBiasAsMatrix(b2);
15733 assertEquals(b1, b2);
15734 final Matrix ma1 = new Matrix(3, 3);
15735 ma1.setSubmatrix(0, 0,
15736 2, 2,
15737 new double[]{sx, myx, mzx,
15738 mxy, sy, mzy,
15739 mxz, myz, sz});
15740 assertEquals(calibrator.getInitialMa(), ma1);
15741 final Matrix ma2 = new Matrix(3, 3);
15742 calibrator.getInitialMa(ma2);
15743 assertEquals(ma1, ma2);
15744 assertSame(calibrator.getMeasurements(), measurements);
15745 assertFalse(calibrator.isCommonAxisUsed());
15746 assertNull(calibrator.getListener());
15747 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15748 assertFalse(calibrator.isReady());
15749 assertFalse(calibrator.isRunning());
15750 assertNull(calibrator.getEstimatedBiases());
15751 assertFalse(calibrator.getEstimatedBiases(null));
15752 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15753 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15754 assertNull(calibrator.getEstimatedBiasFx());
15755 assertNull(calibrator.getEstimatedBiasFy());
15756 assertNull(calibrator.getEstimatedBiasFz());
15757 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15758 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15759 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15760 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15761 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15762 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15763 assertNull(calibrator.getEstimatedMa());
15764 assertNull(calibrator.getEstimatedSx());
15765 assertNull(calibrator.getEstimatedSy());
15766 assertNull(calibrator.getEstimatedSz());
15767 assertNull(calibrator.getEstimatedMxy());
15768 assertNull(calibrator.getEstimatedMxz());
15769 assertNull(calibrator.getEstimatedMyx());
15770 assertNull(calibrator.getEstimatedMyz());
15771 assertNull(calibrator.getEstimatedMzx());
15772 assertNull(calibrator.getEstimatedMzy());
15773 assertNull(calibrator.getEstimatedCovariance());
15774 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15775 assertNotNull(calibrator.getGroundTruthGravityNorm());
15776 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15777 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15778 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15779 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15780 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15781 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15782 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15783
15784
15785 calibrator = null;
15786 try {
15787 calibrator = new KnownGravityNormAccelerometerCalibrator(
15788 -gravityNorm, measurements,
15789 bx, by, bz, sx, sy, sz, mxy, mxz,
15790 myx, myz, mzx, mzy);
15791 fail("IllegalArgumentException expected but not thrown");
15792 } catch (final IllegalArgumentException ignore) {
15793 }
15794 assertNull(calibrator);
15795 }
15796
15797 @Test
15798 public void testConstructor128() throws WrongSizeException {
15799 final Collection<StandardDeviationBodyKinematics> measurements =
15800 Collections.emptyList();
15801
15802 final Matrix ba = generateBa();
15803 final double biasX = ba.getElementAtIndex(0);
15804 final double biasY = ba.getElementAtIndex(1);
15805 final double biasZ = ba.getElementAtIndex(2);
15806
15807 final Acceleration bx = new Acceleration(biasX,
15808 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15809 final Acceleration by = new Acceleration(biasY,
15810 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15811 final Acceleration bz = new Acceleration(biasZ,
15812 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15813
15814 final Matrix ma = generateMaCommonAxis();
15815 final double sx = ma.getElementAt(0, 0);
15816 final double sy = ma.getElementAt(1, 1);
15817 final double sz = ma.getElementAt(2, 2);
15818 final double mxy = ma.getElementAt(0, 1);
15819 final double mxz = ma.getElementAt(0, 2);
15820 final double myx = ma.getElementAt(1, 0);
15821 final double myz = ma.getElementAt(1, 2);
15822 final double mzx = ma.getElementAt(2, 0);
15823 final double mzy = ma.getElementAt(2, 1);
15824
15825 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15826 final double latitude = Math.toRadians(
15827 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15828 final double longitude = Math.toRadians(
15829 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15830 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15831 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15832 final NEDVelocity nedVelocity = new NEDVelocity();
15833 final ECEFPosition ecefPosition = new ECEFPosition();
15834 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15835 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15836 ecefPosition, ecefVelocity);
15837 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15838 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15839 final double gravityNorm = gravity.getNorm();
15840
15841 KnownGravityNormAccelerometerCalibrator calibrator =
15842 new KnownGravityNormAccelerometerCalibrator(
15843 gravityNorm, measurements,
15844 bx, by, bz, sx, sy, sz, mxy, mxz,
15845 myx, myz, mzx, mzy, this);
15846
15847
15848 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15849 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15850 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15851 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15852 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15853 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15854 final Acceleration bx2 = new Acceleration(0.0,
15855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15856 calibrator.getInitialBiasXAsAcceleration(bx2);
15857 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15858 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15859 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15860 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15861 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15862 final Acceleration by2 = new Acceleration(0.0,
15863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15864 calibrator.getInitialBiasYAsAcceleration(by2);
15865 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15866 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15867 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15868 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15869 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15870 final Acceleration bz2 = new Acceleration(0.0,
15871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15872 calibrator.getInitialBiasZAsAcceleration(bz2);
15873 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15874 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15875 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15876 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15877 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15878 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15879 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15880 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15881 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15882 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15883 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15884 final double[] bias1 = calibrator.getInitialBias();
15885 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15886 final double[] bias2 = new double[3];
15887 calibrator.getInitialBias(bias2);
15888 assertArrayEquals(bias1, bias2, 0.0);
15889 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15890 assertEquals(b1, ba);
15891 final Matrix b2 = new Matrix(3, 1);
15892 calibrator.getInitialBiasAsMatrix(b2);
15893 assertEquals(b1, b2);
15894 final Matrix ma1 = new Matrix(3, 3);
15895 ma1.setSubmatrix(0, 0,
15896 2, 2,
15897 new double[]{sx, myx, mzx,
15898 mxy, sy, mzy,
15899 mxz, myz, sz});
15900 assertEquals(calibrator.getInitialMa(), ma1);
15901 final Matrix ma2 = new Matrix(3, 3);
15902 calibrator.getInitialMa(ma2);
15903 assertEquals(ma1, ma2);
15904 assertSame(calibrator.getMeasurements(), measurements);
15905 assertFalse(calibrator.isCommonAxisUsed());
15906 assertSame(calibrator.getListener(), this);
15907 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15908 assertFalse(calibrator.isReady());
15909 assertFalse(calibrator.isRunning());
15910 assertNull(calibrator.getEstimatedBiases());
15911 assertFalse(calibrator.getEstimatedBiases(null));
15912 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15913 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15914 assertNull(calibrator.getEstimatedBiasFx());
15915 assertNull(calibrator.getEstimatedBiasFy());
15916 assertNull(calibrator.getEstimatedBiasFz());
15917 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15918 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15919 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15920 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15921 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15922 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15923 assertNull(calibrator.getEstimatedMa());
15924 assertNull(calibrator.getEstimatedSx());
15925 assertNull(calibrator.getEstimatedSy());
15926 assertNull(calibrator.getEstimatedSz());
15927 assertNull(calibrator.getEstimatedMxy());
15928 assertNull(calibrator.getEstimatedMxz());
15929 assertNull(calibrator.getEstimatedMyx());
15930 assertNull(calibrator.getEstimatedMyz());
15931 assertNull(calibrator.getEstimatedMzx());
15932 assertNull(calibrator.getEstimatedMzy());
15933 assertNull(calibrator.getEstimatedCovariance());
15934 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15935 assertNotNull(calibrator.getGroundTruthGravityNorm());
15936 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15937 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15938 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15939 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15940 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15941 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15942 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15943
15944
15945 calibrator = null;
15946 try {
15947 calibrator = new KnownGravityNormAccelerometerCalibrator(
15948 -gravityNorm, measurements,
15949 bx, by, bz, sx, sy, sz, mxy, mxz,
15950 myx, myz, mzx, mzy, this);
15951 fail("IllegalArgumentException expected but not thrown");
15952 } catch (final IllegalArgumentException ignore) {
15953 }
15954 assertNull(calibrator);
15955 }
15956
15957 @Test
15958 public void testConstructor129() throws WrongSizeException {
15959 final Matrix ba = generateBa();
15960 final double biasX = ba.getElementAtIndex(0);
15961 final double biasY = ba.getElementAtIndex(1);
15962 final double biasZ = ba.getElementAtIndex(2);
15963
15964 final Acceleration bx = new Acceleration(biasX,
15965 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15966 final Acceleration by = new Acceleration(biasY,
15967 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15968 final Acceleration bz = new Acceleration(biasZ,
15969 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15970
15971 final Matrix ma = generateMaCommonAxis();
15972 final double sx = ma.getElementAt(0, 0);
15973 final double sy = ma.getElementAt(1, 1);
15974 final double sz = ma.getElementAt(2, 2);
15975 final double mxy = ma.getElementAt(0, 1);
15976 final double mxz = ma.getElementAt(0, 2);
15977 final double myx = ma.getElementAt(1, 0);
15978 final double myz = ma.getElementAt(1, 2);
15979 final double mzx = ma.getElementAt(2, 0);
15980 final double mzy = ma.getElementAt(2, 1);
15981
15982 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15983 final double latitude = Math.toRadians(
15984 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15985 final double longitude = Math.toRadians(
15986 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15987 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15988 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15989 final NEDVelocity nedVelocity = new NEDVelocity();
15990 final ECEFPosition ecefPosition = new ECEFPosition();
15991 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15992 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15993 ecefPosition, ecefVelocity);
15994 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15995 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15996 final double gravityNorm = gravity.getNorm();
15997
15998 KnownGravityNormAccelerometerCalibrator calibrator =
15999 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16000 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16001 myx, myz, mzx, mzy);
16002
16003
16004 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16005 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16006 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16007 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16008 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16009 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16010 final Acceleration bx2 = new Acceleration(0.0,
16011 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16012 calibrator.getInitialBiasXAsAcceleration(bx2);
16013 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16014 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16015 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16016 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16017 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16018 final Acceleration by2 = new Acceleration(0.0,
16019 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16020 calibrator.getInitialBiasYAsAcceleration(by2);
16021 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16022 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16023 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16024 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16025 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16026 final Acceleration bz2 = new Acceleration(0.0,
16027 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16028 calibrator.getInitialBiasZAsAcceleration(bz2);
16029 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16030 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16031 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16032 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16033 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16034 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16035 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16036 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16037 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16038 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16039 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16040 final double[] bias1 = calibrator.getInitialBias();
16041 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16042 final double[] bias2 = new double[3];
16043 calibrator.getInitialBias(bias2);
16044 assertArrayEquals(bias1, bias2, 0.0);
16045 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16046 assertEquals(b1, ba);
16047 final Matrix b2 = new Matrix(3, 1);
16048 calibrator.getInitialBiasAsMatrix(b2);
16049 assertEquals(b1, b2);
16050 final Matrix ma1 = new Matrix(3, 3);
16051 ma1.setSubmatrix(0, 0,
16052 2, 2,
16053 new double[]{sx, myx, mzx,
16054 mxy, sy, mzy,
16055 mxz, myz, sz});
16056 assertEquals(calibrator.getInitialMa(), ma1);
16057 final Matrix ma2 = new Matrix(3, 3);
16058 calibrator.getInitialMa(ma2);
16059 assertEquals(ma1, ma2);
16060 assertNull(calibrator.getMeasurements());
16061 assertTrue(calibrator.isCommonAxisUsed());
16062 assertNull(calibrator.getListener());
16063 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16064 assertFalse(calibrator.isReady());
16065 assertFalse(calibrator.isRunning());
16066 assertNull(calibrator.getEstimatedBiases());
16067 assertFalse(calibrator.getEstimatedBiases(null));
16068 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16069 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16070 assertNull(calibrator.getEstimatedBiasFx());
16071 assertNull(calibrator.getEstimatedBiasFy());
16072 assertNull(calibrator.getEstimatedBiasFz());
16073 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16074 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16075 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16076 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16077 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16078 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16079 assertNull(calibrator.getEstimatedMa());
16080 assertNull(calibrator.getEstimatedSx());
16081 assertNull(calibrator.getEstimatedSy());
16082 assertNull(calibrator.getEstimatedSz());
16083 assertNull(calibrator.getEstimatedMxy());
16084 assertNull(calibrator.getEstimatedMxz());
16085 assertNull(calibrator.getEstimatedMyx());
16086 assertNull(calibrator.getEstimatedMyz());
16087 assertNull(calibrator.getEstimatedMzx());
16088 assertNull(calibrator.getEstimatedMzy());
16089 assertNull(calibrator.getEstimatedCovariance());
16090 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16091 assertNotNull(calibrator.getGroundTruthGravityNorm());
16092 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16093 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16094 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16095 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16096 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16097 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16098 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16099
16100
16101 calibrator = null;
16102 try {
16103 calibrator = new KnownGravityNormAccelerometerCalibrator(
16104 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
16105 myx, myz, mzx, mzy);
16106 fail("IllegalArgumentException expected but not thrown");
16107 } catch (final IllegalArgumentException ignore) {
16108 }
16109 assertNull(calibrator);
16110 }
16111
16112 @Test
16113 public void testConstructor130() throws WrongSizeException {
16114 final Matrix ba = generateBa();
16115 final double biasX = ba.getElementAtIndex(0);
16116 final double biasY = ba.getElementAtIndex(1);
16117 final double biasZ = ba.getElementAtIndex(2);
16118
16119 final Acceleration bx = new Acceleration(biasX,
16120 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16121 final Acceleration by = new Acceleration(biasY,
16122 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16123 final Acceleration bz = new Acceleration(biasZ,
16124 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16125
16126 final Matrix ma = generateMaCommonAxis();
16127 final double sx = ma.getElementAt(0, 0);
16128 final double sy = ma.getElementAt(1, 1);
16129 final double sz = ma.getElementAt(2, 2);
16130 final double mxy = ma.getElementAt(0, 1);
16131 final double mxz = ma.getElementAt(0, 2);
16132 final double myx = ma.getElementAt(1, 0);
16133 final double myz = ma.getElementAt(1, 2);
16134 final double mzx = ma.getElementAt(2, 0);
16135 final double mzy = ma.getElementAt(2, 1);
16136
16137 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16138 final double latitude = Math.toRadians(
16139 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16140 final double longitude = Math.toRadians(
16141 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16142 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16143 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16144 final NEDVelocity nedVelocity = new NEDVelocity();
16145 final ECEFPosition ecefPosition = new ECEFPosition();
16146 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16147 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16148 ecefPosition, ecefVelocity);
16149 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16150 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16151 final double gravityNorm = gravity.getNorm();
16152
16153 KnownGravityNormAccelerometerCalibrator calibrator =
16154 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16155 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16156 myx, myz, mzx, mzy, this);
16157
16158
16159 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16160 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16161 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16162 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16163 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16164 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16165 final Acceleration bx2 = new Acceleration(0.0,
16166 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16167 calibrator.getInitialBiasXAsAcceleration(bx2);
16168 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16169 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16170 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16171 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16172 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16173 final Acceleration by2 = new Acceleration(0.0,
16174 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16175 calibrator.getInitialBiasYAsAcceleration(by2);
16176 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16177 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16178 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16179 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16180 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16181 final Acceleration bz2 = new Acceleration(0.0,
16182 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16183 calibrator.getInitialBiasZAsAcceleration(bz2);
16184 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16185 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16186 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16187 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16188 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16189 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16190 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16191 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16192 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16193 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16194 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16195 final double[] bias1 = calibrator.getInitialBias();
16196 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16197 final double[] bias2 = new double[3];
16198 calibrator.getInitialBias(bias2);
16199 assertArrayEquals(bias1, bias2, 0.0);
16200 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16201 assertEquals(b1, ba);
16202 final Matrix b2 = new Matrix(3, 1);
16203 calibrator.getInitialBiasAsMatrix(b2);
16204 assertEquals(b1, b2);
16205 final Matrix ma1 = new Matrix(3, 3);
16206 ma1.setSubmatrix(0, 0,
16207 2, 2,
16208 new double[]{sx, myx, mzx,
16209 mxy, sy, mzy,
16210 mxz, myz, sz});
16211 assertEquals(calibrator.getInitialMa(), ma1);
16212 final Matrix ma2 = new Matrix(3, 3);
16213 calibrator.getInitialMa(ma2);
16214 assertEquals(ma1, ma2);
16215 assertNull(calibrator.getMeasurements());
16216 assertTrue(calibrator.isCommonAxisUsed());
16217 assertSame(calibrator.getListener(), this);
16218 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16219 assertFalse(calibrator.isReady());
16220 assertFalse(calibrator.isRunning());
16221 assertNull(calibrator.getEstimatedBiases());
16222 assertFalse(calibrator.getEstimatedBiases(null));
16223 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16224 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16225 assertNull(calibrator.getEstimatedBiasFx());
16226 assertNull(calibrator.getEstimatedBiasFy());
16227 assertNull(calibrator.getEstimatedBiasFz());
16228 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16229 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16230 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16231 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16232 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16233 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16234 assertNull(calibrator.getEstimatedMa());
16235 assertNull(calibrator.getEstimatedSx());
16236 assertNull(calibrator.getEstimatedSy());
16237 assertNull(calibrator.getEstimatedSz());
16238 assertNull(calibrator.getEstimatedMxy());
16239 assertNull(calibrator.getEstimatedMxz());
16240 assertNull(calibrator.getEstimatedMyx());
16241 assertNull(calibrator.getEstimatedMyz());
16242 assertNull(calibrator.getEstimatedMzx());
16243 assertNull(calibrator.getEstimatedMzy());
16244 assertNull(calibrator.getEstimatedCovariance());
16245 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16246 assertNotNull(calibrator.getGroundTruthGravityNorm());
16247 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16248 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16249 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16250 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16251 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16252 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16253 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16254
16255
16256 calibrator = null;
16257 try {
16258 calibrator = new KnownGravityNormAccelerometerCalibrator(
16259 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
16260 myx, myz, mzx, mzy, this);
16261 fail("IllegalArgumentException expected but not thrown");
16262 } catch (final IllegalArgumentException ignore) {
16263 }
16264 assertNull(calibrator);
16265 }
16266
16267 @Test
16268 public void testConstructor131() throws WrongSizeException {
16269 final Collection<StandardDeviationBodyKinematics> measurements =
16270 Collections.emptyList();
16271
16272 final Matrix ba = generateBa();
16273 final double biasX = ba.getElementAtIndex(0);
16274 final double biasY = ba.getElementAtIndex(1);
16275 final double biasZ = ba.getElementAtIndex(2);
16276
16277 final Acceleration bx = new Acceleration(biasX,
16278 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16279 final Acceleration by = new Acceleration(biasY,
16280 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16281 final Acceleration bz = new Acceleration(biasZ,
16282 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16283
16284 final Matrix ma = generateMaCommonAxis();
16285 final double sx = ma.getElementAt(0, 0);
16286 final double sy = ma.getElementAt(1, 1);
16287 final double sz = ma.getElementAt(2, 2);
16288 final double mxy = ma.getElementAt(0, 1);
16289 final double mxz = ma.getElementAt(0, 2);
16290 final double myx = ma.getElementAt(1, 0);
16291 final double myz = ma.getElementAt(1, 2);
16292 final double mzx = ma.getElementAt(2, 0);
16293 final double mzy = ma.getElementAt(2, 1);
16294
16295 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16296 final double latitude = Math.toRadians(
16297 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16298 final double longitude = Math.toRadians(
16299 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16300 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16301 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16302 final NEDVelocity nedVelocity = new NEDVelocity();
16303 final ECEFPosition ecefPosition = new ECEFPosition();
16304 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16305 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16306 ecefPosition, ecefVelocity);
16307 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16308 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16309 final double gravityNorm = gravity.getNorm();
16310
16311 KnownGravityNormAccelerometerCalibrator calibrator =
16312 new KnownGravityNormAccelerometerCalibrator(
16313 gravityNorm, measurements,
16314 true, bx, by, bz, sx, sy, sz,
16315 mxy, mxz, myx, myz, mzx, mzy);
16316
16317
16318 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16319 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16320 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16321 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16322 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16323 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16324 final Acceleration bx2 = new Acceleration(0.0,
16325 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16326 calibrator.getInitialBiasXAsAcceleration(bx2);
16327 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16328 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16329 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16330 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16331 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16332 final Acceleration by2 = new Acceleration(0.0,
16333 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16334 calibrator.getInitialBiasYAsAcceleration(by2);
16335 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16336 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16337 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16338 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16339 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16340 final Acceleration bz2 = new Acceleration(0.0,
16341 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16342 calibrator.getInitialBiasZAsAcceleration(bz2);
16343 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16344 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16345 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16346 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16347 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16348 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16349 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16350 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16351 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16352 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16353 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16354 final double[] bias1 = calibrator.getInitialBias();
16355 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16356 final double[] bias2 = new double[3];
16357 calibrator.getInitialBias(bias2);
16358 assertArrayEquals(bias1, bias2, 0.0);
16359 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16360 assertEquals(b1, ba);
16361 final Matrix b2 = new Matrix(3, 1);
16362 calibrator.getInitialBiasAsMatrix(b2);
16363 assertEquals(b1, b2);
16364 final Matrix ma1 = new Matrix(3, 3);
16365 ma1.setSubmatrix(0, 0,
16366 2, 2,
16367 new double[]{sx, myx, mzx,
16368 mxy, sy, mzy,
16369 mxz, myz, sz});
16370 assertEquals(calibrator.getInitialMa(), ma1);
16371 final Matrix ma2 = new Matrix(3, 3);
16372 calibrator.getInitialMa(ma2);
16373 assertEquals(ma1, ma2);
16374 assertSame(calibrator.getMeasurements(), measurements);
16375 assertTrue(calibrator.isCommonAxisUsed());
16376 assertNull(calibrator.getListener());
16377 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16378 assertFalse(calibrator.isReady());
16379 assertFalse(calibrator.isRunning());
16380 assertNull(calibrator.getEstimatedBiases());
16381 assertFalse(calibrator.getEstimatedBiases(null));
16382 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16383 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16384 assertNull(calibrator.getEstimatedBiasFx());
16385 assertNull(calibrator.getEstimatedBiasFy());
16386 assertNull(calibrator.getEstimatedBiasFz());
16387 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16388 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16389 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16390 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16391 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16392 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16393 assertNull(calibrator.getEstimatedMa());
16394 assertNull(calibrator.getEstimatedSx());
16395 assertNull(calibrator.getEstimatedSy());
16396 assertNull(calibrator.getEstimatedSz());
16397 assertNull(calibrator.getEstimatedMxy());
16398 assertNull(calibrator.getEstimatedMxz());
16399 assertNull(calibrator.getEstimatedMyx());
16400 assertNull(calibrator.getEstimatedMyz());
16401 assertNull(calibrator.getEstimatedMzx());
16402 assertNull(calibrator.getEstimatedMzy());
16403 assertNull(calibrator.getEstimatedCovariance());
16404 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16405 assertNotNull(calibrator.getGroundTruthGravityNorm());
16406 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16407 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16408 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16409 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16410 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16411 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16412 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16413
16414
16415 calibrator = null;
16416 try {
16417 calibrator = new KnownGravityNormAccelerometerCalibrator(
16418 -gravityNorm, measurements,
16419 true, bx, by, bz, sx, sy, sz,
16420 mxy, mxz, myx, myz, mzx, mzy);
16421 fail("IllegalArgumentException expected but not thrown");
16422 } catch (final IllegalArgumentException ignore) {
16423 }
16424 assertNull(calibrator);
16425 }
16426
16427 @Test
16428 public void testConstructor132() throws WrongSizeException {
16429 final Collection<StandardDeviationBodyKinematics> measurements =
16430 Collections.emptyList();
16431
16432 final Matrix ba = generateBa();
16433 final double biasX = ba.getElementAtIndex(0);
16434 final double biasY = ba.getElementAtIndex(1);
16435 final double biasZ = ba.getElementAtIndex(2);
16436
16437 final Acceleration bx = new Acceleration(biasX,
16438 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16439 final Acceleration by = new Acceleration(biasY,
16440 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16441 final Acceleration bz = new Acceleration(biasZ,
16442 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16443
16444 final Matrix ma = generateMaCommonAxis();
16445 final double sx = ma.getElementAt(0, 0);
16446 final double sy = ma.getElementAt(1, 1);
16447 final double sz = ma.getElementAt(2, 2);
16448 final double mxy = ma.getElementAt(0, 1);
16449 final double mxz = ma.getElementAt(0, 2);
16450 final double myx = ma.getElementAt(1, 0);
16451 final double myz = ma.getElementAt(1, 2);
16452 final double mzx = ma.getElementAt(2, 0);
16453 final double mzy = ma.getElementAt(2, 1);
16454
16455 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16456 final double latitude = Math.toRadians(
16457 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16458 final double longitude = Math.toRadians(
16459 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16460 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16461 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16462 final NEDVelocity nedVelocity = new NEDVelocity();
16463 final ECEFPosition ecefPosition = new ECEFPosition();
16464 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16465 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16466 ecefPosition, ecefVelocity);
16467 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16468 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16469 final double gravityNorm = gravity.getNorm();
16470
16471 KnownGravityNormAccelerometerCalibrator calibrator =
16472 new KnownGravityNormAccelerometerCalibrator(
16473 gravityNorm, measurements,
16474 true, bx, by, bz, sx, sy, sz,
16475 mxy, mxz, myx, myz, mzx, mzy, this);
16476
16477
16478 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16479 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16480 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16481 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16482 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16483 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16484 final Acceleration bx2 = new Acceleration(0.0,
16485 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16486 calibrator.getInitialBiasXAsAcceleration(bx2);
16487 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16488 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16489 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16490 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16491 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16492 final Acceleration by2 = new Acceleration(0.0,
16493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16494 calibrator.getInitialBiasYAsAcceleration(by2);
16495 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16496 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16497 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16498 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16499 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16500 final Acceleration bz2 = new Acceleration(0.0,
16501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16502 calibrator.getInitialBiasZAsAcceleration(bz2);
16503 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16504 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16505 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16506 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16507 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16508 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16509 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16510 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16511 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16512 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16513 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16514 final double[] bias1 = calibrator.getInitialBias();
16515 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16516 final double[] bias2 = new double[3];
16517 calibrator.getInitialBias(bias2);
16518 assertArrayEquals(bias1, bias2, 0.0);
16519 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16520 assertEquals(b1, ba);
16521 final Matrix b2 = new Matrix(3, 1);
16522 calibrator.getInitialBiasAsMatrix(b2);
16523 assertEquals(b1, b2);
16524 final Matrix ma1 = new Matrix(3, 3);
16525 ma1.setSubmatrix(0, 0,
16526 2, 2,
16527 new double[]{sx, myx, mzx,
16528 mxy, sy, mzy,
16529 mxz, myz, sz});
16530 assertEquals(calibrator.getInitialMa(), ma1);
16531 final Matrix ma2 = new Matrix(3, 3);
16532 calibrator.getInitialMa(ma2);
16533 assertEquals(ma1, ma2);
16534 assertSame(calibrator.getMeasurements(), measurements);
16535 assertTrue(calibrator.isCommonAxisUsed());
16536 assertSame(calibrator.getListener(), this);
16537 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16538 assertFalse(calibrator.isReady());
16539 assertFalse(calibrator.isRunning());
16540 assertNull(calibrator.getEstimatedBiases());
16541 assertFalse(calibrator.getEstimatedBiases(null));
16542 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16543 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16544 assertNull(calibrator.getEstimatedBiasFx());
16545 assertNull(calibrator.getEstimatedBiasFy());
16546 assertNull(calibrator.getEstimatedBiasFz());
16547 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16548 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16549 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16550 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16551 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16552 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16553 assertNull(calibrator.getEstimatedMa());
16554 assertNull(calibrator.getEstimatedSx());
16555 assertNull(calibrator.getEstimatedSy());
16556 assertNull(calibrator.getEstimatedSz());
16557 assertNull(calibrator.getEstimatedMxy());
16558 assertNull(calibrator.getEstimatedMxz());
16559 assertNull(calibrator.getEstimatedMyx());
16560 assertNull(calibrator.getEstimatedMyz());
16561 assertNull(calibrator.getEstimatedMzx());
16562 assertNull(calibrator.getEstimatedMzy());
16563 assertNull(calibrator.getEstimatedCovariance());
16564 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16565 assertNotNull(calibrator.getGroundTruthGravityNorm());
16566 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16567 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16568 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16569 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16570 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16571 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16572 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16573
16574
16575 calibrator = null;
16576 try {
16577 calibrator = new KnownGravityNormAccelerometerCalibrator(
16578 -gravityNorm, measurements,
16579 true, bx, by, bz, sx, sy, sz,
16580 mxy, mxz, myx, myz, mzx, mzy, this);
16581 fail("IllegalArgumentException expected but not thrown");
16582 } catch (final IllegalArgumentException ignore) {
16583 }
16584 assertNull(calibrator);
16585 }
16586
16587 @Test
16588 public void testConstructor133() throws WrongSizeException {
16589 final Matrix ba = generateBa();
16590 final double[] bias = ba.getBuffer();
16591 final double biasX = ba.getElementAtIndex(0);
16592 final double biasY = ba.getElementAtIndex(1);
16593 final double biasZ = ba.getElementAtIndex(2);
16594
16595 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16596 final double latitude = Math.toRadians(
16597 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16598 final double longitude = Math.toRadians(
16599 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16600 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16601 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16602 final NEDVelocity nedVelocity = new NEDVelocity();
16603 final ECEFPosition ecefPosition = new ECEFPosition();
16604 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16605 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16606 ecefPosition, ecefVelocity);
16607 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16608 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16609 final double gravityNorm = gravity.getNorm();
16610
16611 KnownGravityNormAccelerometerCalibrator calibrator =
16612 new KnownGravityNormAccelerometerCalibrator(
16613 gravityNorm, bias);
16614
16615
16616 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16617 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16618 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16619 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16620 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16621 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16622 final Acceleration bx2 = new Acceleration(0.0,
16623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16624 calibrator.getInitialBiasXAsAcceleration(bx2);
16625 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16626 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16627 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16628 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16629 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16630 final Acceleration by2 = new Acceleration(0.0,
16631 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16632 calibrator.getInitialBiasYAsAcceleration(by2);
16633 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16634 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16635 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16636 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16637 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16638 final Acceleration bz2 = new Acceleration(0.0,
16639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16640 calibrator.getInitialBiasZAsAcceleration(bz2);
16641 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16642 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16643 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16644 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16645 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16646 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16647 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16648 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16649 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16650 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16651 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16652 final double[] bias1 = calibrator.getInitialBias();
16653 assertArrayEquals(bias1, bias, 0.0);
16654 final double[] bias2 = new double[3];
16655 calibrator.getInitialBias(bias2);
16656 assertArrayEquals(bias1, bias2, 0.0);
16657 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16658 assertEquals(b1, ba);
16659 final Matrix b2 = new Matrix(3, 1);
16660 calibrator.getInitialBiasAsMatrix(b2);
16661 assertEquals(b1, b2);
16662 final Matrix ma1 = calibrator.getInitialMa();
16663 assertEquals(ma1, new Matrix(3, 3));
16664 final Matrix ma2 = new Matrix(3, 3);
16665 calibrator.getInitialMa(ma2);
16666 assertEquals(ma1, ma2);
16667 assertNull(calibrator.getMeasurements());
16668 assertFalse(calibrator.isCommonAxisUsed());
16669 assertNull(calibrator.getListener());
16670 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16671 assertFalse(calibrator.isReady());
16672 assertFalse(calibrator.isRunning());
16673 assertNull(calibrator.getEstimatedBiases());
16674 assertFalse(calibrator.getEstimatedBiases(null));
16675 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16676 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16677 assertNull(calibrator.getEstimatedBiasFx());
16678 assertNull(calibrator.getEstimatedBiasFy());
16679 assertNull(calibrator.getEstimatedBiasFz());
16680 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16681 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16682 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16683 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16684 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16685 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16686 assertNull(calibrator.getEstimatedMa());
16687 assertNull(calibrator.getEstimatedSx());
16688 assertNull(calibrator.getEstimatedSy());
16689 assertNull(calibrator.getEstimatedSz());
16690 assertNull(calibrator.getEstimatedMxy());
16691 assertNull(calibrator.getEstimatedMxz());
16692 assertNull(calibrator.getEstimatedMyx());
16693 assertNull(calibrator.getEstimatedMyz());
16694 assertNull(calibrator.getEstimatedMzx());
16695 assertNull(calibrator.getEstimatedMzy());
16696 assertNull(calibrator.getEstimatedCovariance());
16697 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16698 assertNotNull(calibrator.getGroundTruthGravityNorm());
16699 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16700 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16701 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16702 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16703 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16704 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16705 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16706
16707
16708 calibrator = null;
16709 try {
16710 calibrator = new KnownGravityNormAccelerometerCalibrator(
16711 -gravityNorm, bias);
16712 fail("IllegalArgumentException expected but not thrown");
16713 } catch (final IllegalArgumentException ignore) {
16714 }
16715 try {
16716 calibrator = new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16717 new double[1]);
16718 fail("IllegalArgumentException expected but not thrown");
16719 } catch (final IllegalArgumentException ignore) {
16720 }
16721 assertNull(calibrator);
16722 }
16723
16724 @Test
16725 public void testConstructor134() throws WrongSizeException {
16726 final Matrix ba = generateBa();
16727 final double[] bias = ba.getBuffer();
16728 final double biasX = ba.getElementAtIndex(0);
16729 final double biasY = ba.getElementAtIndex(1);
16730 final double biasZ = ba.getElementAtIndex(2);
16731
16732 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16733 final double latitude = Math.toRadians(
16734 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16735 final double longitude = Math.toRadians(
16736 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16737 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16738 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16739 final NEDVelocity nedVelocity = new NEDVelocity();
16740 final ECEFPosition ecefPosition = new ECEFPosition();
16741 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16742 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16743 ecefPosition, ecefVelocity);
16744 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16745 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16746 final double gravityNorm = gravity.getNorm();
16747
16748 KnownGravityNormAccelerometerCalibrator calibrator =
16749 new KnownGravityNormAccelerometerCalibrator(
16750 gravityNorm, bias, this);
16751
16752
16753 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16754 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16755 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16756 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16757 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16758 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16759 final Acceleration bx2 = new Acceleration(0.0,
16760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16761 calibrator.getInitialBiasXAsAcceleration(bx2);
16762 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16763 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16764 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16765 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16766 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16767 final Acceleration by2 = new Acceleration(0.0,
16768 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16769 calibrator.getInitialBiasYAsAcceleration(by2);
16770 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16771 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16772 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16773 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16774 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16775 final Acceleration bz2 = new Acceleration(0.0,
16776 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16777 calibrator.getInitialBiasZAsAcceleration(bz2);
16778 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16779 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16780 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16781 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16782 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16783 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16784 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16785 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16786 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16787 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16788 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16789 final double[] bias1 = calibrator.getInitialBias();
16790 assertArrayEquals(bias1, bias, 0.0);
16791 final double[] bias2 = new double[3];
16792 calibrator.getInitialBias(bias2);
16793 assertArrayEquals(bias1, bias2, 0.0);
16794 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16795 assertEquals(b1, ba);
16796 final Matrix b2 = new Matrix(3, 1);
16797 calibrator.getInitialBiasAsMatrix(b2);
16798 assertEquals(b1, b2);
16799 final Matrix ma1 = calibrator.getInitialMa();
16800 assertEquals(ma1, new Matrix(3, 3));
16801 final Matrix ma2 = new Matrix(3, 3);
16802 calibrator.getInitialMa(ma2);
16803 assertEquals(ma1, ma2);
16804 assertNull(calibrator.getMeasurements());
16805 assertFalse(calibrator.isCommonAxisUsed());
16806 assertSame(calibrator.getListener(), this);
16807 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16808 assertFalse(calibrator.isReady());
16809 assertFalse(calibrator.isRunning());
16810 assertNull(calibrator.getEstimatedBiases());
16811 assertFalse(calibrator.getEstimatedBiases(null));
16812 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16813 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16814 assertNull(calibrator.getEstimatedBiasFx());
16815 assertNull(calibrator.getEstimatedBiasFy());
16816 assertNull(calibrator.getEstimatedBiasFz());
16817 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16818 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16819 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16820 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16821 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16822 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16823 assertNull(calibrator.getEstimatedMa());
16824 assertNull(calibrator.getEstimatedSx());
16825 assertNull(calibrator.getEstimatedSy());
16826 assertNull(calibrator.getEstimatedSz());
16827 assertNull(calibrator.getEstimatedMxy());
16828 assertNull(calibrator.getEstimatedMxz());
16829 assertNull(calibrator.getEstimatedMyx());
16830 assertNull(calibrator.getEstimatedMyz());
16831 assertNull(calibrator.getEstimatedMzx());
16832 assertNull(calibrator.getEstimatedMzy());
16833 assertNull(calibrator.getEstimatedCovariance());
16834 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16835 assertNotNull(calibrator.getGroundTruthGravityNorm());
16836 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16837 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16838 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16839 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16840 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16841 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16842 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16843
16844
16845 calibrator = null;
16846 try {
16847 calibrator = new KnownGravityNormAccelerometerCalibrator(
16848 -gravityNorm, bias, this);
16849 fail("IllegalArgumentException expected but not thrown");
16850 } catch (final IllegalArgumentException ignore) {
16851 }
16852 try {
16853 calibrator = new KnownGravityNormAccelerometerCalibrator(
16854 gravityNorm, new double[1], this);
16855 fail("IllegalArgumentException expected but not thrown");
16856 } catch (final IllegalArgumentException ignore) {
16857 }
16858 assertNull(calibrator);
16859 }
16860
16861 @Test
16862 public void testConstructor135() throws WrongSizeException {
16863 final Collection<StandardDeviationBodyKinematics> measurements =
16864 Collections.emptyList();
16865
16866 final Matrix ba = generateBa();
16867 final double[] bias = ba.getBuffer();
16868 final double biasX = ba.getElementAtIndex(0);
16869 final double biasY = ba.getElementAtIndex(1);
16870 final double biasZ = ba.getElementAtIndex(2);
16871
16872 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16873 final double latitude = Math.toRadians(
16874 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16875 final double longitude = Math.toRadians(
16876 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16877 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16878 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16879 final NEDVelocity nedVelocity = new NEDVelocity();
16880 final ECEFPosition ecefPosition = new ECEFPosition();
16881 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16882 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16883 ecefPosition, ecefVelocity);
16884 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16885 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16886 final double gravityNorm = gravity.getNorm();
16887
16888 KnownGravityNormAccelerometerCalibrator calibrator =
16889 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16890 measurements, bias);
16891
16892
16893 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16894 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16895 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16896 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16897 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16898 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16899 final Acceleration bx2 = new Acceleration(0.0,
16900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16901 calibrator.getInitialBiasXAsAcceleration(bx2);
16902 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16903 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16904 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16905 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16906 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16907 final Acceleration by2 = new Acceleration(0.0,
16908 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16909 calibrator.getInitialBiasYAsAcceleration(by2);
16910 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16911 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16912 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16913 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16914 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16915 final Acceleration bz2 = new Acceleration(0.0,
16916 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16917 calibrator.getInitialBiasZAsAcceleration(bz2);
16918 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16919 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16920 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16921 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16922 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16923 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16924 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16925 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16926 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16927 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16928 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16929 final double[] bias1 = calibrator.getInitialBias();
16930 assertArrayEquals(bias1, bias, 0.0);
16931 final double[] bias2 = new double[3];
16932 calibrator.getInitialBias(bias2);
16933 assertArrayEquals(bias1, bias2, 0.0);
16934 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16935 assertEquals(b1, ba);
16936 final Matrix b2 = new Matrix(3, 1);
16937 calibrator.getInitialBiasAsMatrix(b2);
16938 assertEquals(b1, b2);
16939 final Matrix ma1 = calibrator.getInitialMa();
16940 assertEquals(ma1, new Matrix(3, 3));
16941 final Matrix ma2 = new Matrix(3, 3);
16942 calibrator.getInitialMa(ma2);
16943 assertEquals(ma1, ma2);
16944 assertSame(calibrator.getMeasurements(), measurements);
16945 assertFalse(calibrator.isCommonAxisUsed());
16946 assertNull(calibrator.getListener());
16947 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16948 assertFalse(calibrator.isReady());
16949 assertFalse(calibrator.isRunning());
16950 assertNull(calibrator.getEstimatedBiases());
16951 assertFalse(calibrator.getEstimatedBiases(null));
16952 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16953 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16954 assertNull(calibrator.getEstimatedBiasFx());
16955 assertNull(calibrator.getEstimatedBiasFy());
16956 assertNull(calibrator.getEstimatedBiasFz());
16957 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16958 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16959 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16960 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16961 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16962 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16963 assertNull(calibrator.getEstimatedMa());
16964 assertNull(calibrator.getEstimatedSx());
16965 assertNull(calibrator.getEstimatedSy());
16966 assertNull(calibrator.getEstimatedSz());
16967 assertNull(calibrator.getEstimatedMxy());
16968 assertNull(calibrator.getEstimatedMxz());
16969 assertNull(calibrator.getEstimatedMyx());
16970 assertNull(calibrator.getEstimatedMyz());
16971 assertNull(calibrator.getEstimatedMzx());
16972 assertNull(calibrator.getEstimatedMzy());
16973 assertNull(calibrator.getEstimatedCovariance());
16974 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16975 assertNotNull(calibrator.getGroundTruthGravityNorm());
16976 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16977 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16978 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16979 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16980 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16981 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16982 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16983
16984
16985 calibrator = null;
16986 try {
16987 calibrator = new KnownGravityNormAccelerometerCalibrator(
16988 -gravityNorm, measurements, bias);
16989 fail("IllegalArgumentException expected but not thrown");
16990 } catch (final IllegalArgumentException ignore) {
16991 }
16992 try {
16993 calibrator = new KnownGravityNormAccelerometerCalibrator(
16994 gravityNorm, measurements, new double[1]);
16995 fail("IllegalArgumentException expected but not thrown");
16996 } catch (final IllegalArgumentException ignore) {
16997 }
16998 assertNull(calibrator);
16999 }
17000
17001 @Test
17002 public void testConstructor136() throws WrongSizeException {
17003 final Collection<StandardDeviationBodyKinematics> measurements =
17004 Collections.emptyList();
17005
17006 final Matrix ba = generateBa();
17007 final double[] bias = ba.getBuffer();
17008 final double biasX = ba.getElementAtIndex(0);
17009 final double biasY = ba.getElementAtIndex(1);
17010 final double biasZ = ba.getElementAtIndex(2);
17011
17012 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17013 final double latitude = Math.toRadians(
17014 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17015 final double longitude = Math.toRadians(
17016 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17017 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17018 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17019 final NEDVelocity nedVelocity = new NEDVelocity();
17020 final ECEFPosition ecefPosition = new ECEFPosition();
17021 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17022 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17023 ecefPosition, ecefVelocity);
17024 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17025 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17026 final double gravityNorm = gravity.getNorm();
17027
17028 KnownGravityNormAccelerometerCalibrator calibrator =
17029 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17030 measurements, bias, this);
17031
17032
17033 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17034 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17035 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17036 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17037 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17038 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17039 final Acceleration bx2 = new Acceleration(0.0,
17040 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17041 calibrator.getInitialBiasXAsAcceleration(bx2);
17042 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17043 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17044 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17045 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17046 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17047 final Acceleration by2 = new Acceleration(0.0,
17048 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17049 calibrator.getInitialBiasYAsAcceleration(by2);
17050 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17051 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17052 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17053 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17054 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17055 final Acceleration bz2 = new Acceleration(0.0,
17056 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17057 calibrator.getInitialBiasZAsAcceleration(bz2);
17058 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17059 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17060 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17061 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17062 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17063 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17064 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17065 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17066 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17067 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17068 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17069 final double[] bias1 = calibrator.getInitialBias();
17070 assertArrayEquals(bias1, bias, 0.0);
17071 final double[] bias2 = new double[3];
17072 calibrator.getInitialBias(bias2);
17073 assertArrayEquals(bias1, bias2, 0.0);
17074 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17075 assertEquals(b1, ba);
17076 final Matrix b2 = new Matrix(3, 1);
17077 calibrator.getInitialBiasAsMatrix(b2);
17078 assertEquals(b1, b2);
17079 final Matrix ma1 = calibrator.getInitialMa();
17080 assertEquals(ma1, new Matrix(3, 3));
17081 final Matrix ma2 = new Matrix(3, 3);
17082 calibrator.getInitialMa(ma2);
17083 assertEquals(ma1, ma2);
17084 assertSame(calibrator.getMeasurements(), measurements);
17085 assertFalse(calibrator.isCommonAxisUsed());
17086 assertSame(calibrator.getListener(), this);
17087 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17088 assertFalse(calibrator.isReady());
17089 assertFalse(calibrator.isRunning());
17090 assertNull(calibrator.getEstimatedBiases());
17091 assertFalse(calibrator.getEstimatedBiases(null));
17092 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17093 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17094 assertNull(calibrator.getEstimatedBiasFx());
17095 assertNull(calibrator.getEstimatedBiasFy());
17096 assertNull(calibrator.getEstimatedBiasFz());
17097 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17098 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17099 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17100 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17101 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17102 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17103 assertNull(calibrator.getEstimatedMa());
17104 assertNull(calibrator.getEstimatedSx());
17105 assertNull(calibrator.getEstimatedSy());
17106 assertNull(calibrator.getEstimatedSz());
17107 assertNull(calibrator.getEstimatedMxy());
17108 assertNull(calibrator.getEstimatedMxz());
17109 assertNull(calibrator.getEstimatedMyx());
17110 assertNull(calibrator.getEstimatedMyz());
17111 assertNull(calibrator.getEstimatedMzx());
17112 assertNull(calibrator.getEstimatedMzy());
17113 assertNull(calibrator.getEstimatedCovariance());
17114 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17115 assertNotNull(calibrator.getGroundTruthGravityNorm());
17116 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17117 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17118 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17119 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17120 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17121 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17122 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17123
17124
17125 calibrator = null;
17126 try {
17127 calibrator = new KnownGravityNormAccelerometerCalibrator(
17128 -gravityNorm, measurements, bias, this);
17129 fail("IllegalArgumentException expected but not thrown");
17130 } catch (final IllegalArgumentException ignore) {
17131 }
17132 try {
17133 calibrator = new KnownGravityNormAccelerometerCalibrator(
17134 gravityNorm, measurements, new double[1], this);
17135 fail("IllegalArgumentException expected but not thrown");
17136 } catch (final IllegalArgumentException ignore) {
17137 }
17138 assertNull(calibrator);
17139 }
17140
17141 @Test
17142 public void testConstructor137() throws WrongSizeException {
17143 final Matrix ba = generateBa();
17144 final double[] bias = ba.getBuffer();
17145 final double biasX = ba.getElementAtIndex(0);
17146 final double biasY = ba.getElementAtIndex(1);
17147 final double biasZ = ba.getElementAtIndex(2);
17148
17149 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17150 final double latitude = Math.toRadians(
17151 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17152 final double longitude = Math.toRadians(
17153 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17154 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17155 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17156 final NEDVelocity nedVelocity = new NEDVelocity();
17157 final ECEFPosition ecefPosition = new ECEFPosition();
17158 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17159 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17160 ecefPosition, ecefVelocity);
17161 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17162 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17163 final double gravityNorm = gravity.getNorm();
17164
17165 KnownGravityNormAccelerometerCalibrator calibrator =
17166 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17167 true, bias);
17168
17169
17170 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17171 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17172 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17173 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17174 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17175 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17176 final Acceleration bx2 = new Acceleration(0.0,
17177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17178 calibrator.getInitialBiasXAsAcceleration(bx2);
17179 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17180 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17181 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17182 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17183 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17184 final Acceleration by2 = new Acceleration(0.0,
17185 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17186 calibrator.getInitialBiasYAsAcceleration(by2);
17187 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17188 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17189 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17190 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17191 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17192 final Acceleration bz2 = new Acceleration(0.0,
17193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17194 calibrator.getInitialBiasZAsAcceleration(bz2);
17195 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17196 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17197 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17198 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17199 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17200 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17201 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17202 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17203 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17204 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17205 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17206 final double[] bias1 = calibrator.getInitialBias();
17207 assertArrayEquals(bias1, bias, 0.0);
17208 final double[] bias2 = new double[3];
17209 calibrator.getInitialBias(bias2);
17210 assertArrayEquals(bias1, bias2, 0.0);
17211 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17212 assertEquals(b1, ba);
17213 final Matrix b2 = new Matrix(3, 1);
17214 calibrator.getInitialBiasAsMatrix(b2);
17215 assertEquals(b1, b2);
17216 final Matrix ma1 = calibrator.getInitialMa();
17217 assertEquals(ma1, new Matrix(3, 3));
17218 final Matrix ma2 = new Matrix(3, 3);
17219 calibrator.getInitialMa(ma2);
17220 assertEquals(ma1, ma2);
17221 assertNull(calibrator.getMeasurements());
17222 assertTrue(calibrator.isCommonAxisUsed());
17223 assertNull(calibrator.getListener());
17224 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17225 assertFalse(calibrator.isReady());
17226 assertFalse(calibrator.isRunning());
17227 assertNull(calibrator.getEstimatedBiases());
17228 assertFalse(calibrator.getEstimatedBiases(null));
17229 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17230 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17231 assertNull(calibrator.getEstimatedBiasFx());
17232 assertNull(calibrator.getEstimatedBiasFy());
17233 assertNull(calibrator.getEstimatedBiasFz());
17234 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17235 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17236 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17237 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17238 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17239 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17240 assertNull(calibrator.getEstimatedMa());
17241 assertNull(calibrator.getEstimatedSx());
17242 assertNull(calibrator.getEstimatedSy());
17243 assertNull(calibrator.getEstimatedSz());
17244 assertNull(calibrator.getEstimatedMxy());
17245 assertNull(calibrator.getEstimatedMxz());
17246 assertNull(calibrator.getEstimatedMyx());
17247 assertNull(calibrator.getEstimatedMyz());
17248 assertNull(calibrator.getEstimatedMzx());
17249 assertNull(calibrator.getEstimatedMzy());
17250 assertNull(calibrator.getEstimatedCovariance());
17251 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17252 assertNotNull(calibrator.getGroundTruthGravityNorm());
17253 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17254 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17255 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17256 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17257 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17258 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17259 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17260
17261
17262 calibrator = null;
17263 try {
17264 calibrator = new KnownGravityNormAccelerometerCalibrator(
17265 -gravityNorm, true, bias);
17266 fail("IllegalArgumentException expected but not thrown");
17267 } catch (final IllegalArgumentException ignore) {
17268 }
17269 try {
17270 calibrator = new KnownGravityNormAccelerometerCalibrator(
17271 gravityNorm, true, new double[1]);
17272 fail("IllegalArgumentException expected but not thrown");
17273 } catch (final IllegalArgumentException ignore) {
17274 }
17275 assertNull(calibrator);
17276 }
17277
17278 @Test
17279 public void testConstructor138() throws WrongSizeException {
17280 final Matrix ba = generateBa();
17281 final double[] bias = ba.getBuffer();
17282 final double biasX = ba.getElementAtIndex(0);
17283 final double biasY = ba.getElementAtIndex(1);
17284 final double biasZ = ba.getElementAtIndex(2);
17285
17286 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17287 final double latitude = Math.toRadians(
17288 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17289 final double longitude = Math.toRadians(
17290 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17291 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17292 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17293 final NEDVelocity nedVelocity = new NEDVelocity();
17294 final ECEFPosition ecefPosition = new ECEFPosition();
17295 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17296 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17297 ecefPosition, ecefVelocity);
17298 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17299 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17300 final double gravityNorm = gravity.getNorm();
17301
17302 KnownGravityNormAccelerometerCalibrator calibrator =
17303 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17304 true, bias, this);
17305
17306
17307 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17308 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17309 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17310 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17311 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17312 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17313 final Acceleration bx2 = new Acceleration(0.0,
17314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17315 calibrator.getInitialBiasXAsAcceleration(bx2);
17316 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17317 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17318 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17319 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17320 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17321 final Acceleration by2 = new Acceleration(0.0,
17322 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17323 calibrator.getInitialBiasYAsAcceleration(by2);
17324 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17325 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17326 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17327 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17328 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17329 final Acceleration bz2 = new Acceleration(0.0,
17330 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17331 calibrator.getInitialBiasZAsAcceleration(bz2);
17332 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17333 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17334 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17335 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17336 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17337 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17338 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17339 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17340 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17341 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17342 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17343 final double[] bias1 = calibrator.getInitialBias();
17344 assertArrayEquals(bias1, bias, 0.0);
17345 final double[] bias2 = new double[3];
17346 calibrator.getInitialBias(bias2);
17347 assertArrayEquals(bias1, bias2, 0.0);
17348 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17349 assertEquals(b1, ba);
17350 final Matrix b2 = new Matrix(3, 1);
17351 calibrator.getInitialBiasAsMatrix(b2);
17352 assertEquals(b1, b2);
17353 final Matrix ma1 = calibrator.getInitialMa();
17354 assertEquals(ma1, new Matrix(3, 3));
17355 final Matrix ma2 = new Matrix(3, 3);
17356 calibrator.getInitialMa(ma2);
17357 assertEquals(ma1, ma2);
17358 assertNull(calibrator.getMeasurements());
17359 assertTrue(calibrator.isCommonAxisUsed());
17360 assertSame(calibrator.getListener(), this);
17361 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17362 assertFalse(calibrator.isReady());
17363 assertFalse(calibrator.isRunning());
17364 assertNull(calibrator.getEstimatedBiases());
17365 assertFalse(calibrator.getEstimatedBiases(null));
17366 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17367 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17368 assertNull(calibrator.getEstimatedBiasFx());
17369 assertNull(calibrator.getEstimatedBiasFy());
17370 assertNull(calibrator.getEstimatedBiasFz());
17371 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17372 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17373 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17374 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17375 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17376 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17377 assertNull(calibrator.getEstimatedMa());
17378 assertNull(calibrator.getEstimatedSx());
17379 assertNull(calibrator.getEstimatedSy());
17380 assertNull(calibrator.getEstimatedSz());
17381 assertNull(calibrator.getEstimatedMxy());
17382 assertNull(calibrator.getEstimatedMxz());
17383 assertNull(calibrator.getEstimatedMyx());
17384 assertNull(calibrator.getEstimatedMyz());
17385 assertNull(calibrator.getEstimatedMzx());
17386 assertNull(calibrator.getEstimatedMzy());
17387 assertNull(calibrator.getEstimatedCovariance());
17388 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17389 assertNotNull(calibrator.getGroundTruthGravityNorm());
17390 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17391 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17392 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17393 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17394 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17395 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17396 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17397
17398
17399 calibrator = null;
17400 try {
17401 calibrator = new KnownGravityNormAccelerometerCalibrator(
17402 -gravityNorm, true, bias, this);
17403 fail("IllegalArgumentException expected but not thrown");
17404 } catch (final IllegalArgumentException ignore) {
17405 }
17406 try {
17407 calibrator = new KnownGravityNormAccelerometerCalibrator(
17408 gravityNorm, true, new double[1],
17409 this);
17410 fail("IllegalArgumentException expected but not thrown");
17411 } catch (final IllegalArgumentException ignore) {
17412 }
17413 assertNull(calibrator);
17414 }
17415
17416 @Test
17417 public void testConstructor139() throws WrongSizeException {
17418 final Collection<StandardDeviationBodyKinematics> measurements =
17419 Collections.emptyList();
17420
17421 final Matrix ba = generateBa();
17422 final double[] bias = ba.getBuffer();
17423 final double biasX = ba.getElementAtIndex(0);
17424 final double biasY = ba.getElementAtIndex(1);
17425 final double biasZ = ba.getElementAtIndex(2);
17426
17427 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17428 final double latitude = Math.toRadians(
17429 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17430 final double longitude = Math.toRadians(
17431 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17432 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17433 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17434 final NEDVelocity nedVelocity = new NEDVelocity();
17435 final ECEFPosition ecefPosition = new ECEFPosition();
17436 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17437 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17438 ecefPosition, ecefVelocity);
17439 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17440 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17441 final double gravityNorm = gravity.getNorm();
17442
17443 KnownGravityNormAccelerometerCalibrator calibrator =
17444 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17445 measurements, true, bias);
17446
17447
17448 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17449 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17450 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17451 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17452 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17453 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17454 final Acceleration bx2 = new Acceleration(0.0,
17455 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17456 calibrator.getInitialBiasXAsAcceleration(bx2);
17457 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17458 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17459 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17460 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17461 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17462 final Acceleration by2 = new Acceleration(0.0,
17463 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17464 calibrator.getInitialBiasYAsAcceleration(by2);
17465 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17466 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17467 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17468 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17469 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17470 final Acceleration bz2 = new Acceleration(0.0,
17471 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17472 calibrator.getInitialBiasZAsAcceleration(bz2);
17473 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17474 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17475 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17476 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17477 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17478 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17479 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17480 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17481 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17482 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17483 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17484 final double[] bias1 = calibrator.getInitialBias();
17485 assertArrayEquals(bias1, bias, 0.0);
17486 final double[] bias2 = new double[3];
17487 calibrator.getInitialBias(bias2);
17488 assertArrayEquals(bias1, bias2, 0.0);
17489 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17490 assertEquals(b1, ba);
17491 final Matrix b2 = new Matrix(3, 1);
17492 calibrator.getInitialBiasAsMatrix(b2);
17493 assertEquals(b1, b2);
17494 final Matrix ma1 = calibrator.getInitialMa();
17495 assertEquals(ma1, new Matrix(3, 3));
17496 final Matrix ma2 = new Matrix(3, 3);
17497 calibrator.getInitialMa(ma2);
17498 assertEquals(ma1, ma2);
17499 assertSame(calibrator.getMeasurements(), measurements);
17500 assertTrue(calibrator.isCommonAxisUsed());
17501 assertNull(calibrator.getListener());
17502 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17503 assertFalse(calibrator.isReady());
17504 assertFalse(calibrator.isRunning());
17505 assertNull(calibrator.getEstimatedBiases());
17506 assertFalse(calibrator.getEstimatedBiases(null));
17507 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17508 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17509 assertNull(calibrator.getEstimatedBiasFx());
17510 assertNull(calibrator.getEstimatedBiasFy());
17511 assertNull(calibrator.getEstimatedBiasFz());
17512 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17513 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17514 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17515 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17516 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17517 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17518 assertNull(calibrator.getEstimatedMa());
17519 assertNull(calibrator.getEstimatedSx());
17520 assertNull(calibrator.getEstimatedSy());
17521 assertNull(calibrator.getEstimatedSz());
17522 assertNull(calibrator.getEstimatedMxy());
17523 assertNull(calibrator.getEstimatedMxz());
17524 assertNull(calibrator.getEstimatedMyx());
17525 assertNull(calibrator.getEstimatedMyz());
17526 assertNull(calibrator.getEstimatedMzx());
17527 assertNull(calibrator.getEstimatedMzy());
17528 assertNull(calibrator.getEstimatedCovariance());
17529 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17530 assertNotNull(calibrator.getGroundTruthGravityNorm());
17531 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17532 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17533 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17534 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17535 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17536 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17537 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17538
17539
17540 calibrator = null;
17541 try {
17542 calibrator = new KnownGravityNormAccelerometerCalibrator(
17543 -gravityNorm, measurements, true, bias);
17544 fail("IllegalArgumentException expected but not thrown");
17545 } catch (final IllegalArgumentException ignore) {
17546 }
17547 try {
17548 calibrator = new KnownGravityNormAccelerometerCalibrator(
17549 gravityNorm, measurements, true,
17550 new double[1]);
17551 fail("IllegalArgumentException expected but not thrown");
17552 } catch (final IllegalArgumentException ignore) {
17553 }
17554 assertNull(calibrator);
17555 }
17556
17557 @Test
17558 public void testConstructor140() throws WrongSizeException {
17559 final Collection<StandardDeviationBodyKinematics> measurements =
17560 Collections.emptyList();
17561
17562 final Matrix ba = generateBa();
17563 final double[] bias = ba.getBuffer();
17564 final double biasX = ba.getElementAtIndex(0);
17565 final double biasY = ba.getElementAtIndex(1);
17566 final double biasZ = ba.getElementAtIndex(2);
17567
17568 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17569 final double latitude = Math.toRadians(
17570 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17571 final double longitude = Math.toRadians(
17572 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17573 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17574 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17575 final NEDVelocity nedVelocity = new NEDVelocity();
17576 final ECEFPosition ecefPosition = new ECEFPosition();
17577 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17578 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17579 ecefPosition, ecefVelocity);
17580 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17581 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17582 final double gravityNorm = gravity.getNorm();
17583
17584 KnownGravityNormAccelerometerCalibrator calibrator =
17585 new KnownGravityNormAccelerometerCalibrator(
17586 gravityNorm, measurements,
17587 true, bias, this);
17588
17589
17590 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17591 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17592 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17593 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17594 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17595 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17596 final Acceleration bx2 = new Acceleration(0.0,
17597 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17598 calibrator.getInitialBiasXAsAcceleration(bx2);
17599 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17600 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17601 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17602 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17603 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17604 final Acceleration by2 = new Acceleration(0.0,
17605 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17606 calibrator.getInitialBiasYAsAcceleration(by2);
17607 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17608 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17609 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17610 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17611 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17612 final Acceleration bz2 = new Acceleration(0.0,
17613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17614 calibrator.getInitialBiasZAsAcceleration(bz2);
17615 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17616 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17617 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17618 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17619 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17620 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17621 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17622 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17623 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17624 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17625 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17626 final double[] bias1 = calibrator.getInitialBias();
17627 assertArrayEquals(bias1, bias, 0.0);
17628 final double[] bias2 = new double[3];
17629 calibrator.getInitialBias(bias2);
17630 assertArrayEquals(bias1, bias2, 0.0);
17631 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17632 assertEquals(b1, ba);
17633 final Matrix b2 = new Matrix(3, 1);
17634 calibrator.getInitialBiasAsMatrix(b2);
17635 assertEquals(b1, b2);
17636 final Matrix ma1 = calibrator.getInitialMa();
17637 assertEquals(ma1, new Matrix(3, 3));
17638 final Matrix ma2 = new Matrix(3, 3);
17639 calibrator.getInitialMa(ma2);
17640 assertEquals(ma1, ma2);
17641 assertSame(calibrator.getMeasurements(), measurements);
17642 assertTrue(calibrator.isCommonAxisUsed());
17643 assertSame(calibrator.getListener(), this);
17644 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17645 assertFalse(calibrator.isReady());
17646 assertFalse(calibrator.isRunning());
17647 assertNull(calibrator.getEstimatedBiases());
17648 assertFalse(calibrator.getEstimatedBiases(null));
17649 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17650 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17651 assertNull(calibrator.getEstimatedBiasFx());
17652 assertNull(calibrator.getEstimatedBiasFy());
17653 assertNull(calibrator.getEstimatedBiasFz());
17654 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17655 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17656 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17657 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17658 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17659 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17660 assertNull(calibrator.getEstimatedMa());
17661 assertNull(calibrator.getEstimatedSx());
17662 assertNull(calibrator.getEstimatedSy());
17663 assertNull(calibrator.getEstimatedSz());
17664 assertNull(calibrator.getEstimatedMxy());
17665 assertNull(calibrator.getEstimatedMxz());
17666 assertNull(calibrator.getEstimatedMyx());
17667 assertNull(calibrator.getEstimatedMyz());
17668 assertNull(calibrator.getEstimatedMzx());
17669 assertNull(calibrator.getEstimatedMzy());
17670 assertNull(calibrator.getEstimatedCovariance());
17671 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17672 assertNotNull(calibrator.getGroundTruthGravityNorm());
17673 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17674 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17675 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17676 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17677 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17678 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17679 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17680
17681
17682 calibrator = null;
17683 try {
17684 calibrator = new KnownGravityNormAccelerometerCalibrator(
17685 -gravityNorm, measurements,
17686 true, bias, this);
17687 fail("IllegalArgumentException expected but not thrown");
17688 } catch (final IllegalArgumentException ignore) {
17689 }
17690 try {
17691 calibrator = new KnownGravityNormAccelerometerCalibrator(
17692 gravityNorm, measurements, true,
17693 new double[1], this);
17694 fail("IllegalArgumentException expected but not thrown");
17695 } catch (final IllegalArgumentException ignore) {
17696 }
17697 assertNull(calibrator);
17698 }
17699
17700 @Test
17701 public void testConstructor141() throws WrongSizeException {
17702 final Matrix ba = generateBa();
17703 final double[] bias = ba.getBuffer();
17704 final double biasX = ba.getElementAtIndex(0);
17705 final double biasY = ba.getElementAtIndex(1);
17706 final double biasZ = ba.getElementAtIndex(2);
17707
17708 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17709 final double latitude = Math.toRadians(
17710 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17711 final double longitude = Math.toRadians(
17712 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17713 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17714 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17715 final NEDVelocity nedVelocity = new NEDVelocity();
17716 final ECEFPosition ecefPosition = new ECEFPosition();
17717 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17718 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17719 ecefPosition, ecefVelocity);
17720 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17721 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17722 final double gravityNorm = gravity.getNorm();
17723
17724 KnownGravityNormAccelerometerCalibrator calibrator =
17725 new KnownGravityNormAccelerometerCalibrator(
17726 gravityNorm, ba);
17727
17728
17729 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17730 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17731 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17732 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17733 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17734 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17735 final Acceleration bx2 = new Acceleration(0.0,
17736 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17737 calibrator.getInitialBiasXAsAcceleration(bx2);
17738 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17739 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17740 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17741 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17742 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17743 final Acceleration by2 = new Acceleration(0.0,
17744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17745 calibrator.getInitialBiasYAsAcceleration(by2);
17746 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17747 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17748 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17749 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17750 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17751 final Acceleration bz2 = new Acceleration(0.0,
17752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17753 calibrator.getInitialBiasZAsAcceleration(bz2);
17754 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17755 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17756 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17757 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17758 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17759 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17760 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17761 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17762 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17763 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17764 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17765 final double[] bias1 = calibrator.getInitialBias();
17766 assertArrayEquals(bias1, bias, 0.0);
17767 final double[] bias2 = new double[3];
17768 calibrator.getInitialBias(bias2);
17769 assertArrayEquals(bias1, bias2, 0.0);
17770 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17771 assertEquals(b1, ba);
17772 final Matrix b2 = new Matrix(3, 1);
17773 calibrator.getInitialBiasAsMatrix(b2);
17774 assertEquals(b1, b2);
17775 final Matrix ma1 = calibrator.getInitialMa();
17776 assertEquals(ma1, new Matrix(3, 3));
17777 final Matrix ma2 = new Matrix(3, 3);
17778 calibrator.getInitialMa(ma2);
17779 assertEquals(ma1, ma2);
17780 assertNull(calibrator.getMeasurements());
17781 assertFalse(calibrator.isCommonAxisUsed());
17782 assertNull(calibrator.getListener());
17783 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17784 assertFalse(calibrator.isReady());
17785 assertFalse(calibrator.isRunning());
17786 assertNull(calibrator.getEstimatedBiases());
17787 assertFalse(calibrator.getEstimatedBiases(null));
17788 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17789 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17790 assertNull(calibrator.getEstimatedBiasFx());
17791 assertNull(calibrator.getEstimatedBiasFy());
17792 assertNull(calibrator.getEstimatedBiasFz());
17793 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17794 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17795 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17796 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17797 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17798 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17799 assertNull(calibrator.getEstimatedMa());
17800 assertNull(calibrator.getEstimatedSx());
17801 assertNull(calibrator.getEstimatedSy());
17802 assertNull(calibrator.getEstimatedSz());
17803 assertNull(calibrator.getEstimatedMxy());
17804 assertNull(calibrator.getEstimatedMxz());
17805 assertNull(calibrator.getEstimatedMyx());
17806 assertNull(calibrator.getEstimatedMyz());
17807 assertNull(calibrator.getEstimatedMzx());
17808 assertNull(calibrator.getEstimatedMzy());
17809 assertNull(calibrator.getEstimatedCovariance());
17810 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17811 assertNotNull(calibrator.getGroundTruthGravityNorm());
17812 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17813 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17814 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17815 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17816 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17817 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17818 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17819
17820
17821 calibrator = null;
17822 try {
17823 calibrator = new KnownGravityNormAccelerometerCalibrator(
17824 -gravityNorm, ba);
17825 fail("IllegalArgumentException expected but not thrown");
17826 } catch (final IllegalArgumentException ignore) {
17827 }
17828 try {
17829 calibrator = new KnownGravityNormAccelerometerCalibrator(
17830 gravityNorm, new Matrix(1, 1));
17831 fail("IllegalArgumentException expected but not thrown");
17832 } catch (final IllegalArgumentException ignore) {
17833 }
17834 try {
17835 calibrator = new KnownGravityNormAccelerometerCalibrator(
17836 gravityNorm, new Matrix(1, 3));
17837 fail("IllegalArgumentException expected but not thrown");
17838 } catch (final IllegalArgumentException ignore) {
17839 }
17840 assertNull(calibrator);
17841 }
17842
17843 @Test
17844 public void testConstructor142() throws WrongSizeException {
17845 final Matrix ba = generateBa();
17846 final double[] bias = ba.getBuffer();
17847 final double biasX = ba.getElementAtIndex(0);
17848 final double biasY = ba.getElementAtIndex(1);
17849 final double biasZ = ba.getElementAtIndex(2);
17850
17851 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17852 final double latitude = Math.toRadians(
17853 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17854 final double longitude = Math.toRadians(
17855 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17856 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17857 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17858 final NEDVelocity nedVelocity = new NEDVelocity();
17859 final ECEFPosition ecefPosition = new ECEFPosition();
17860 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17861 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17862 ecefPosition, ecefVelocity);
17863 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17864 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17865 final double gravityNorm = gravity.getNorm();
17866
17867 KnownGravityNormAccelerometerCalibrator calibrator =
17868 new KnownGravityNormAccelerometerCalibrator(
17869 gravityNorm, ba, this);
17870
17871
17872 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17873 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17874 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17875 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17876 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17877 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17878 final Acceleration bx2 = new Acceleration(0.0,
17879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17880 calibrator.getInitialBiasXAsAcceleration(bx2);
17881 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17882 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17883 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17884 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17885 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17886 final Acceleration by2 = new Acceleration(0.0,
17887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17888 calibrator.getInitialBiasYAsAcceleration(by2);
17889 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17890 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17891 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17892 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17893 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17894 final Acceleration bz2 = new Acceleration(0.0,
17895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17896 calibrator.getInitialBiasZAsAcceleration(bz2);
17897 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17898 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17899 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17900 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17901 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17902 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17903 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17904 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17905 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17906 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17907 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17908 final double[] bias1 = calibrator.getInitialBias();
17909 assertArrayEquals(bias1, bias, 0.0);
17910 final double[] bias2 = new double[3];
17911 calibrator.getInitialBias(bias2);
17912 assertArrayEquals(bias1, bias2, 0.0);
17913 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17914 assertEquals(b1, ba);
17915 final Matrix b2 = new Matrix(3, 1);
17916 calibrator.getInitialBiasAsMatrix(b2);
17917 assertEquals(b1, b2);
17918 final Matrix ma1 = calibrator.getInitialMa();
17919 assertEquals(ma1, new Matrix(3, 3));
17920 final Matrix ma2 = new Matrix(3, 3);
17921 calibrator.getInitialMa(ma2);
17922 assertEquals(ma1, ma2);
17923 assertNull(calibrator.getMeasurements());
17924 assertFalse(calibrator.isCommonAxisUsed());
17925 assertSame(calibrator.getListener(), this);
17926 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17927 assertFalse(calibrator.isReady());
17928 assertFalse(calibrator.isRunning());
17929 assertNull(calibrator.getEstimatedBiases());
17930 assertFalse(calibrator.getEstimatedBiases(null));
17931 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17932 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17933 assertNull(calibrator.getEstimatedBiasFx());
17934 assertNull(calibrator.getEstimatedBiasFy());
17935 assertNull(calibrator.getEstimatedBiasFz());
17936 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17937 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17938 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17939 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17940 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17941 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17942 assertNull(calibrator.getEstimatedMa());
17943 assertNull(calibrator.getEstimatedSx());
17944 assertNull(calibrator.getEstimatedSy());
17945 assertNull(calibrator.getEstimatedSz());
17946 assertNull(calibrator.getEstimatedMxy());
17947 assertNull(calibrator.getEstimatedMxz());
17948 assertNull(calibrator.getEstimatedMyx());
17949 assertNull(calibrator.getEstimatedMyz());
17950 assertNull(calibrator.getEstimatedMzx());
17951 assertNull(calibrator.getEstimatedMzy());
17952 assertNull(calibrator.getEstimatedCovariance());
17953 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17954 assertNotNull(calibrator.getGroundTruthGravityNorm());
17955 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17956 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17957 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17958 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17959 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17960 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17961 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17962
17963
17964 calibrator = null;
17965 try {
17966 calibrator = new KnownGravityNormAccelerometerCalibrator(
17967 -gravityNorm, ba, this);
17968 fail("IllegalArgumentException expected but not thrown");
17969 } catch (final IllegalArgumentException ignore) {
17970 }
17971 try {
17972 calibrator = new KnownGravityNormAccelerometerCalibrator(
17973 gravityNorm, new Matrix(1, 1), this);
17974 fail("IllegalArgumentException expected but not thrown");
17975 } catch (final IllegalArgumentException ignore) {
17976 }
17977 try {
17978 calibrator = new KnownGravityNormAccelerometerCalibrator(
17979 gravityNorm, new Matrix(1, 3),
17980 this);
17981 fail("IllegalArgumentException expected but not thrown");
17982 } catch (final IllegalArgumentException ignore) {
17983 }
17984 assertNull(calibrator);
17985 }
17986
17987 @Test
17988 public void testConstructor143() throws WrongSizeException {
17989 final Collection<StandardDeviationBodyKinematics> measurements =
17990 Collections.emptyList();
17991
17992 final Matrix ba = generateBa();
17993 final double[] bias = ba.getBuffer();
17994 final double biasX = ba.getElementAtIndex(0);
17995 final double biasY = ba.getElementAtIndex(1);
17996 final double biasZ = ba.getElementAtIndex(2);
17997
17998 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17999 final double latitude = Math.toRadians(
18000 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18001 final double longitude = Math.toRadians(
18002 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18003 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18004 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18005 final NEDVelocity nedVelocity = new NEDVelocity();
18006 final ECEFPosition ecefPosition = new ECEFPosition();
18007 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18008 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18009 ecefPosition, ecefVelocity);
18010 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18011 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18012 final double gravityNorm = gravity.getNorm();
18013
18014 KnownGravityNormAccelerometerCalibrator calibrator =
18015 new KnownGravityNormAccelerometerCalibrator(
18016 gravityNorm, measurements, ba);
18017
18018
18019 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18020 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18021 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18022 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18023 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18024 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18025 final Acceleration bx2 = new Acceleration(0.0,
18026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18027 calibrator.getInitialBiasXAsAcceleration(bx2);
18028 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18029 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18030 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18031 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18032 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18033 final Acceleration by2 = new Acceleration(0.0,
18034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18035 calibrator.getInitialBiasYAsAcceleration(by2);
18036 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18037 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18038 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18039 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18040 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18041 final Acceleration bz2 = new Acceleration(0.0,
18042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18043 calibrator.getInitialBiasZAsAcceleration(bz2);
18044 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18045 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18046 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18047 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18048 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18049 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18050 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18051 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18052 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18053 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18054 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18055 final double[] bias1 = calibrator.getInitialBias();
18056 assertArrayEquals(bias1, bias, 0.0);
18057 final double[] bias2 = new double[3];
18058 calibrator.getInitialBias(bias2);
18059 assertArrayEquals(bias1, bias2, 0.0);
18060 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18061 assertEquals(b1, ba);
18062 final Matrix b2 = new Matrix(3, 1);
18063 calibrator.getInitialBiasAsMatrix(b2);
18064 assertEquals(b1, b2);
18065 final Matrix ma1 = calibrator.getInitialMa();
18066 assertEquals(ma1, new Matrix(3, 3));
18067 final Matrix ma2 = new Matrix(3, 3);
18068 calibrator.getInitialMa(ma2);
18069 assertEquals(ma1, ma2);
18070 assertSame(calibrator.getMeasurements(), measurements);
18071 assertFalse(calibrator.isCommonAxisUsed());
18072 assertNull(calibrator.getListener());
18073 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18074 assertFalse(calibrator.isReady());
18075 assertFalse(calibrator.isRunning());
18076 assertNull(calibrator.getEstimatedBiases());
18077 assertFalse(calibrator.getEstimatedBiases(null));
18078 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18079 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18080 assertNull(calibrator.getEstimatedBiasFx());
18081 assertNull(calibrator.getEstimatedBiasFy());
18082 assertNull(calibrator.getEstimatedBiasFz());
18083 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18084 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18085 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18086 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18087 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18088 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18089 assertNull(calibrator.getEstimatedMa());
18090 assertNull(calibrator.getEstimatedSx());
18091 assertNull(calibrator.getEstimatedSy());
18092 assertNull(calibrator.getEstimatedSz());
18093 assertNull(calibrator.getEstimatedMxy());
18094 assertNull(calibrator.getEstimatedMxz());
18095 assertNull(calibrator.getEstimatedMyx());
18096 assertNull(calibrator.getEstimatedMyz());
18097 assertNull(calibrator.getEstimatedMzx());
18098 assertNull(calibrator.getEstimatedMzy());
18099 assertNull(calibrator.getEstimatedCovariance());
18100 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18101 assertNotNull(calibrator.getGroundTruthGravityNorm());
18102 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18103 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18104 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18105 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18106 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18107 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18108 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18109
18110
18111 calibrator = null;
18112 try {
18113 calibrator = new KnownGravityNormAccelerometerCalibrator(
18114 -gravityNorm, measurements, ba);
18115 fail("IllegalArgumentException expected but not thrown");
18116 } catch (final IllegalArgumentException ignore) {
18117 }
18118 try {
18119 calibrator = new KnownGravityNormAccelerometerCalibrator(
18120 gravityNorm, measurements,
18121 new Matrix(1, 1));
18122 fail("IllegalArgumentException expected but not thrown");
18123 } catch (final IllegalArgumentException ignore) {
18124 }
18125 try {
18126 calibrator = new KnownGravityNormAccelerometerCalibrator(
18127 gravityNorm, measurements,
18128 new Matrix(1, 3));
18129 fail("IllegalArgumentException expected but not thrown");
18130 } catch (final IllegalArgumentException ignore) {
18131 }
18132 assertNull(calibrator);
18133 }
18134
18135 @Test
18136 public void testConstructor144() throws WrongSizeException {
18137 final Collection<StandardDeviationBodyKinematics> measurements =
18138 Collections.emptyList();
18139
18140 final Matrix ba = generateBa();
18141 final double[] bias = ba.getBuffer();
18142 final double biasX = ba.getElementAtIndex(0);
18143 final double biasY = ba.getElementAtIndex(1);
18144 final double biasZ = ba.getElementAtIndex(2);
18145
18146 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18147 final double latitude = Math.toRadians(
18148 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18149 final double longitude = Math.toRadians(
18150 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18151 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18152 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18153 final NEDVelocity nedVelocity = new NEDVelocity();
18154 final ECEFPosition ecefPosition = new ECEFPosition();
18155 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18156 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18157 ecefPosition, ecefVelocity);
18158 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18159 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18160 final double gravityNorm = gravity.getNorm();
18161
18162 KnownGravityNormAccelerometerCalibrator calibrator =
18163 new KnownGravityNormAccelerometerCalibrator(
18164 gravityNorm, measurements, ba, this);
18165
18166
18167 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18168 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18169 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18170 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18171 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18172 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18173 final Acceleration bx2 = new Acceleration(0.0,
18174 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18175 calibrator.getInitialBiasXAsAcceleration(bx2);
18176 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18177 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18178 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18179 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18180 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18181 final Acceleration by2 = new Acceleration(0.0,
18182 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18183 calibrator.getInitialBiasYAsAcceleration(by2);
18184 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18185 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18186 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18187 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18188 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18189 final Acceleration bz2 = new Acceleration(0.0,
18190 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18191 calibrator.getInitialBiasZAsAcceleration(bz2);
18192 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18193 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18194 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18195 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18196 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18197 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18198 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18199 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18200 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18201 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18202 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18203 final double[] bias1 = calibrator.getInitialBias();
18204 assertArrayEquals(bias1, bias, 0.0);
18205 final double[] bias2 = new double[3];
18206 calibrator.getInitialBias(bias2);
18207 assertArrayEquals(bias1, bias2, 0.0);
18208 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18209 assertEquals(b1, ba);
18210 final Matrix b2 = new Matrix(3, 1);
18211 calibrator.getInitialBiasAsMatrix(b2);
18212 assertEquals(b1, b2);
18213 final Matrix ma1 = calibrator.getInitialMa();
18214 assertEquals(ma1, new Matrix(3, 3));
18215 final Matrix ma2 = new Matrix(3, 3);
18216 calibrator.getInitialMa(ma2);
18217 assertEquals(ma1, ma2);
18218 assertSame(calibrator.getMeasurements(), measurements);
18219 assertFalse(calibrator.isCommonAxisUsed());
18220 assertSame(calibrator.getListener(), this);
18221 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18222 assertFalse(calibrator.isReady());
18223 assertFalse(calibrator.isRunning());
18224 assertNull(calibrator.getEstimatedBiases());
18225 assertFalse(calibrator.getEstimatedBiases(null));
18226 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18227 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18228 assertNull(calibrator.getEstimatedBiasFx());
18229 assertNull(calibrator.getEstimatedBiasFy());
18230 assertNull(calibrator.getEstimatedBiasFz());
18231 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18232 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18233 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18234 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18235 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18236 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18237 assertNull(calibrator.getEstimatedMa());
18238 assertNull(calibrator.getEstimatedSx());
18239 assertNull(calibrator.getEstimatedSy());
18240 assertNull(calibrator.getEstimatedSz());
18241 assertNull(calibrator.getEstimatedMxy());
18242 assertNull(calibrator.getEstimatedMxz());
18243 assertNull(calibrator.getEstimatedMyx());
18244 assertNull(calibrator.getEstimatedMyz());
18245 assertNull(calibrator.getEstimatedMzx());
18246 assertNull(calibrator.getEstimatedMzy());
18247 assertNull(calibrator.getEstimatedCovariance());
18248 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18249 assertNotNull(calibrator.getGroundTruthGravityNorm());
18250 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18251 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18252 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18253 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18254 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18255 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18256 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18257
18258
18259 calibrator = null;
18260 try {
18261 calibrator = new KnownGravityNormAccelerometerCalibrator(
18262 -gravityNorm, measurements, ba, this);
18263 fail("IllegalArgumentException expected but not thrown");
18264 } catch (final IllegalArgumentException ignore) {
18265 }
18266 try {
18267 calibrator = new KnownGravityNormAccelerometerCalibrator(
18268 gravityNorm, measurements,
18269 new Matrix(1, 1), this);
18270 fail("IllegalArgumentException expected but not thrown");
18271 } catch (final IllegalArgumentException ignore) {
18272 }
18273 try {
18274 calibrator = new KnownGravityNormAccelerometerCalibrator(
18275 gravityNorm, measurements,
18276 new Matrix(1, 3), this);
18277 fail("IllegalArgumentException expected but not thrown");
18278 } catch (final IllegalArgumentException ignore) {
18279 }
18280 assertNull(calibrator);
18281 }
18282
18283 @Test
18284 public void testConstructor145() throws WrongSizeException {
18285 final Matrix ba = generateBa();
18286 final double[] bias = ba.getBuffer();
18287 final double biasX = ba.getElementAtIndex(0);
18288 final double biasY = ba.getElementAtIndex(1);
18289 final double biasZ = ba.getElementAtIndex(2);
18290
18291 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18292 final double latitude = Math.toRadians(
18293 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18294 final double longitude = Math.toRadians(
18295 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18296 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18297 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18298 final NEDVelocity nedVelocity = new NEDVelocity();
18299 final ECEFPosition ecefPosition = new ECEFPosition();
18300 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18301 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18302 ecefPosition, ecefVelocity);
18303 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305 final double gravityNorm = gravity.getNorm();
18306
18307 KnownGravityNormAccelerometerCalibrator calibrator =
18308 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
18309 true, ba);
18310
18311
18312 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18313 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18314 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18315 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18316 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18317 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18318 final Acceleration bx2 = new Acceleration(0.0,
18319 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18320 calibrator.getInitialBiasXAsAcceleration(bx2);
18321 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18322 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18323 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18324 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18325 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18326 final Acceleration by2 = new Acceleration(0.0,
18327 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18328 calibrator.getInitialBiasYAsAcceleration(by2);
18329 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18330 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18331 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18332 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18333 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18334 final Acceleration bz2 = new Acceleration(0.0,
18335 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18336 calibrator.getInitialBiasZAsAcceleration(bz2);
18337 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18338 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18339 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18340 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18341 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18342 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18343 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18344 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18345 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18346 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18347 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18348 final double[] bias1 = calibrator.getInitialBias();
18349 assertArrayEquals(bias1, bias, 0.0);
18350 final double[] bias2 = new double[3];
18351 calibrator.getInitialBias(bias2);
18352 assertArrayEquals(bias1, bias2, 0.0);
18353 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18354 assertEquals(b1, ba);
18355 final Matrix b2 = new Matrix(3, 1);
18356 calibrator.getInitialBiasAsMatrix(b2);
18357 assertEquals(b1, b2);
18358 final Matrix ma1 = calibrator.getInitialMa();
18359 assertEquals(ma1, new Matrix(3, 3));
18360 final Matrix ma2 = new Matrix(3, 3);
18361 calibrator.getInitialMa(ma2);
18362 assertEquals(ma1, ma2);
18363 assertNull(calibrator.getMeasurements());
18364 assertTrue(calibrator.isCommonAxisUsed());
18365 assertNull(calibrator.getListener());
18366 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18367 assertFalse(calibrator.isReady());
18368 assertFalse(calibrator.isRunning());
18369 assertNull(calibrator.getEstimatedBiases());
18370 assertFalse(calibrator.getEstimatedBiases(null));
18371 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18372 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18373 assertNull(calibrator.getEstimatedBiasFx());
18374 assertNull(calibrator.getEstimatedBiasFy());
18375 assertNull(calibrator.getEstimatedBiasFz());
18376 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18377 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18378 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18379 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18380 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18381 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18382 assertNull(calibrator.getEstimatedMa());
18383 assertNull(calibrator.getEstimatedSx());
18384 assertNull(calibrator.getEstimatedSy());
18385 assertNull(calibrator.getEstimatedSz());
18386 assertNull(calibrator.getEstimatedMxy());
18387 assertNull(calibrator.getEstimatedMxz());
18388 assertNull(calibrator.getEstimatedMyx());
18389 assertNull(calibrator.getEstimatedMyz());
18390 assertNull(calibrator.getEstimatedMzx());
18391 assertNull(calibrator.getEstimatedMzy());
18392 assertNull(calibrator.getEstimatedCovariance());
18393 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18394 assertNotNull(calibrator.getGroundTruthGravityNorm());
18395 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18396 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18397 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18398 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18399 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18400 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18401 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18402
18403
18404 calibrator = null;
18405 try {
18406 calibrator = new KnownGravityNormAccelerometerCalibrator(
18407 -gravityNorm, true, ba);
18408 fail("IllegalArgumentException expected but not thrown");
18409 } catch (final IllegalArgumentException ignore) {
18410 }
18411 try {
18412 calibrator = new KnownGravityNormAccelerometerCalibrator(
18413 gravityNorm, true,
18414 new Matrix(1, 1));
18415 fail("IllegalArgumentException expected but not thrown");
18416 } catch (final IllegalArgumentException ignore) {
18417 }
18418 try {
18419 calibrator = new KnownGravityNormAccelerometerCalibrator(
18420 gravityNorm, true,
18421 new Matrix(1, 3));
18422 fail("IllegalArgumentException expected but not thrown");
18423 } catch (final IllegalArgumentException ignore) {
18424 }
18425 assertNull(calibrator);
18426 }
18427
18428 @Test
18429 public void testConstructor146() throws WrongSizeException {
18430 final Matrix ba = generateBa();
18431 final double[] bias = ba.getBuffer();
18432 final double biasX = ba.getElementAtIndex(0);
18433 final double biasY = ba.getElementAtIndex(1);
18434 final double biasZ = ba.getElementAtIndex(2);
18435
18436 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18437 final double latitude = Math.toRadians(
18438 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18439 final double longitude = Math.toRadians(
18440 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18441 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18442 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18443 final NEDVelocity nedVelocity = new NEDVelocity();
18444 final ECEFPosition ecefPosition = new ECEFPosition();
18445 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18446 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18447 ecefPosition, ecefVelocity);
18448 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18449 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18450 final double gravityNorm = gravity.getNorm();
18451
18452 KnownGravityNormAccelerometerCalibrator calibrator =
18453 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
18454 true, ba, this);
18455
18456
18457 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18458 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18459 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18460 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18461 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18462 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18463 final Acceleration bx2 = new Acceleration(0.0,
18464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18465 calibrator.getInitialBiasXAsAcceleration(bx2);
18466 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18467 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18468 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18469 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18470 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18471 final Acceleration by2 = new Acceleration(0.0,
18472 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18473 calibrator.getInitialBiasYAsAcceleration(by2);
18474 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18475 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18476 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18477 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18478 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18479 final Acceleration bz2 = new Acceleration(0.0,
18480 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18481 calibrator.getInitialBiasZAsAcceleration(bz2);
18482 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18483 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18484 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18485 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18486 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18487 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18488 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18489 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18490 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18491 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18492 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18493 final double[] bias1 = calibrator.getInitialBias();
18494 assertArrayEquals(bias1, bias, 0.0);
18495 final double[] bias2 = new double[3];
18496 calibrator.getInitialBias(bias2);
18497 assertArrayEquals(bias1, bias2, 0.0);
18498 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18499 assertEquals(b1, ba);
18500 final Matrix b2 = new Matrix(3, 1);
18501 calibrator.getInitialBiasAsMatrix(b2);
18502 assertEquals(b1, b2);
18503 final Matrix ma1 = calibrator.getInitialMa();
18504 assertEquals(ma1, new Matrix(3, 3));
18505 final Matrix ma2 = new Matrix(3, 3);
18506 calibrator.getInitialMa(ma2);
18507 assertEquals(ma1, ma2);
18508 assertNull(calibrator.getMeasurements());
18509 assertTrue(calibrator.isCommonAxisUsed());
18510 assertSame(calibrator.getListener(), this);
18511 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18512 assertFalse(calibrator.isReady());
18513 assertFalse(calibrator.isRunning());
18514 assertNull(calibrator.getEstimatedBiases());
18515 assertFalse(calibrator.getEstimatedBiases(null));
18516 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18517 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18518 assertNull(calibrator.getEstimatedBiasFx());
18519 assertNull(calibrator.getEstimatedBiasFy());
18520 assertNull(calibrator.getEstimatedBiasFz());
18521 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18522 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18523 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18524 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18525 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18526 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18527 assertNull(calibrator.getEstimatedMa());
18528 assertNull(calibrator.getEstimatedSx());
18529 assertNull(calibrator.getEstimatedSy());
18530 assertNull(calibrator.getEstimatedSz());
18531 assertNull(calibrator.getEstimatedMxy());
18532 assertNull(calibrator.getEstimatedMxz());
18533 assertNull(calibrator.getEstimatedMyx());
18534 assertNull(calibrator.getEstimatedMyz());
18535 assertNull(calibrator.getEstimatedMzx());
18536 assertNull(calibrator.getEstimatedMzy());
18537 assertNull(calibrator.getEstimatedCovariance());
18538 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18539 assertNotNull(calibrator.getGroundTruthGravityNorm());
18540 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18541 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18542 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18543 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18544 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18545 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18546 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18547
18548
18549 calibrator = null;
18550 try {
18551 calibrator = new KnownGravityNormAccelerometerCalibrator(
18552 -gravityNorm, true, ba, this);
18553 fail("IllegalArgumentException expected but not thrown");
18554 } catch (final IllegalArgumentException ignore) {
18555 }
18556 try {
18557 calibrator = new KnownGravityNormAccelerometerCalibrator(
18558 gravityNorm, true, new Matrix(1, 1),
18559 this);
18560 fail("IllegalArgumentException expected but not thrown");
18561 } catch (final IllegalArgumentException ignore) {
18562 }
18563 try {
18564 calibrator = new KnownGravityNormAccelerometerCalibrator(
18565 gravityNorm, true, new Matrix(1, 3),
18566 this);
18567 fail("IllegalArgumentException expected but not thrown");
18568 } catch (final IllegalArgumentException ignore) {
18569 }
18570 assertNull(calibrator);
18571 }
18572
18573 @Test
18574 public void testConstructor147() throws WrongSizeException {
18575 final Collection<StandardDeviationBodyKinematics> measurements =
18576 Collections.emptyList();
18577
18578 final Matrix ba = generateBa();
18579 final double[] bias = ba.getBuffer();
18580 final double biasX = ba.getElementAtIndex(0);
18581 final double biasY = ba.getElementAtIndex(1);
18582 final double biasZ = ba.getElementAtIndex(2);
18583
18584 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18585 final double latitude = Math.toRadians(
18586 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18587 final double longitude = Math.toRadians(
18588 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18589 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18590 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18591 final NEDVelocity nedVelocity = new NEDVelocity();
18592 final ECEFPosition ecefPosition = new ECEFPosition();
18593 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18594 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18595 ecefPosition, ecefVelocity);
18596 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18597 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18598 final double gravityNorm = gravity.getNorm();
18599
18600 KnownGravityNormAccelerometerCalibrator calibrator =
18601 new KnownGravityNormAccelerometerCalibrator(
18602 gravityNorm, measurements,
18603 true, ba);
18604
18605
18606 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18607 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18608 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18609 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18610 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18611 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18612 final Acceleration bx2 = new Acceleration(0.0,
18613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18614 calibrator.getInitialBiasXAsAcceleration(bx2);
18615 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18616 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18617 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18618 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18619 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18620 final Acceleration by2 = new Acceleration(0.0,
18621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18622 calibrator.getInitialBiasYAsAcceleration(by2);
18623 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18624 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18625 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18626 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18627 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18628 final Acceleration bz2 = new Acceleration(0.0,
18629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18630 calibrator.getInitialBiasZAsAcceleration(bz2);
18631 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18632 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18633 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18634 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18635 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18636 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18637 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18638 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18639 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18640 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18641 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18642 final double[] bias1 = calibrator.getInitialBias();
18643 assertArrayEquals(bias1, bias, 0.0);
18644 final double[] bias2 = new double[3];
18645 calibrator.getInitialBias(bias2);
18646 assertArrayEquals(bias1, bias2, 0.0);
18647 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18648 assertEquals(b1, ba);
18649 final Matrix b2 = new Matrix(3, 1);
18650 calibrator.getInitialBiasAsMatrix(b2);
18651 assertEquals(b1, b2);
18652 final Matrix ma1 = calibrator.getInitialMa();
18653 assertEquals(ma1, new Matrix(3, 3));
18654 final Matrix ma2 = new Matrix(3, 3);
18655 calibrator.getInitialMa(ma2);
18656 assertEquals(ma1, ma2);
18657 assertSame(calibrator.getMeasurements(), measurements);
18658 assertTrue(calibrator.isCommonAxisUsed());
18659 assertNull(calibrator.getListener());
18660 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18661 assertFalse(calibrator.isReady());
18662 assertFalse(calibrator.isRunning());
18663 assertNull(calibrator.getEstimatedBiases());
18664 assertFalse(calibrator.getEstimatedBiases(null));
18665 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18666 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18667 assertNull(calibrator.getEstimatedBiasFx());
18668 assertNull(calibrator.getEstimatedBiasFy());
18669 assertNull(calibrator.getEstimatedBiasFz());
18670 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18671 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18672 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18673 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18674 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18675 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18676 assertNull(calibrator.getEstimatedMa());
18677 assertNull(calibrator.getEstimatedSx());
18678 assertNull(calibrator.getEstimatedSy());
18679 assertNull(calibrator.getEstimatedSz());
18680 assertNull(calibrator.getEstimatedMxy());
18681 assertNull(calibrator.getEstimatedMxz());
18682 assertNull(calibrator.getEstimatedMyx());
18683 assertNull(calibrator.getEstimatedMyz());
18684 assertNull(calibrator.getEstimatedMzx());
18685 assertNull(calibrator.getEstimatedMzy());
18686 assertNull(calibrator.getEstimatedCovariance());
18687 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18688 assertNotNull(calibrator.getGroundTruthGravityNorm());
18689 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18690 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18691 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18692 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18693 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18694 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18695 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18696
18697
18698 calibrator = null;
18699 try {
18700 calibrator = new KnownGravityNormAccelerometerCalibrator(
18701 -gravityNorm, measurements,
18702 true, ba);
18703 fail("IllegalArgumentException expected but not thrown");
18704 } catch (final IllegalArgumentException ignore) {
18705 }
18706 try {
18707 calibrator = new KnownGravityNormAccelerometerCalibrator(
18708 gravityNorm, measurements, true,
18709 new Matrix(1, 1));
18710 fail("IllegalArgumentException expected but not thrown");
18711 } catch (final IllegalArgumentException ignore) {
18712 }
18713 try {
18714 calibrator = new KnownGravityNormAccelerometerCalibrator(
18715 gravityNorm, measurements, true,
18716 new Matrix(1, 3));
18717 fail("IllegalArgumentException expected but not thrown");
18718 } catch (final IllegalArgumentException ignore) {
18719 }
18720 assertNull(calibrator);
18721 }
18722
18723 @Test
18724 public void testConstructor148() throws WrongSizeException {
18725 final Collection<StandardDeviationBodyKinematics> measurements =
18726 Collections.emptyList();
18727
18728 final Matrix ba = generateBa();
18729 final double[] bias = ba.getBuffer();
18730 final double biasX = ba.getElementAtIndex(0);
18731 final double biasY = ba.getElementAtIndex(1);
18732 final double biasZ = ba.getElementAtIndex(2);
18733
18734 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18735 final double latitude = Math.toRadians(
18736 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18737 final double longitude = Math.toRadians(
18738 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18739 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18740 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18741 final NEDVelocity nedVelocity = new NEDVelocity();
18742 final ECEFPosition ecefPosition = new ECEFPosition();
18743 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18744 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18745 ecefPosition, ecefVelocity);
18746 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18747 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18748 final double gravityNorm = gravity.getNorm();
18749
18750 KnownGravityNormAccelerometerCalibrator calibrator =
18751 new KnownGravityNormAccelerometerCalibrator(
18752 gravityNorm, measurements,
18753 true, ba, this);
18754
18755
18756 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18757 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18758 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18759 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18760 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18761 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18762 final Acceleration bx2 = new Acceleration(0.0,
18763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18764 calibrator.getInitialBiasXAsAcceleration(bx2);
18765 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18766 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18767 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18768 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18769 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18770 final Acceleration by2 = new Acceleration(0.0,
18771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18772 calibrator.getInitialBiasYAsAcceleration(by2);
18773 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18774 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18775 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18776 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18777 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18778 final Acceleration bz2 = new Acceleration(0.0,
18779 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18780 calibrator.getInitialBiasZAsAcceleration(bz2);
18781 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18782 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18783 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18784 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18785 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18786 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18787 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18788 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18789 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18790 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18791 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18792 final double[] bias1 = calibrator.getInitialBias();
18793 assertArrayEquals(bias1, bias, 0.0);
18794 final double[] bias2 = new double[3];
18795 calibrator.getInitialBias(bias2);
18796 assertArrayEquals(bias1, bias2, 0.0);
18797 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18798 assertEquals(b1, ba);
18799 final Matrix b2 = new Matrix(3, 1);
18800 calibrator.getInitialBiasAsMatrix(b2);
18801 assertEquals(b1, b2);
18802 final Matrix ma1 = calibrator.getInitialMa();
18803 assertEquals(ma1, new Matrix(3, 3));
18804 final Matrix ma2 = new Matrix(3, 3);
18805 calibrator.getInitialMa(ma2);
18806 assertEquals(ma1, ma2);
18807 assertSame(calibrator.getMeasurements(), measurements);
18808 assertTrue(calibrator.isCommonAxisUsed());
18809 assertSame(calibrator.getListener(), this);
18810 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18811 assertFalse(calibrator.isReady());
18812 assertFalse(calibrator.isRunning());
18813 assertNull(calibrator.getEstimatedBiases());
18814 assertFalse(calibrator.getEstimatedBiases(null));
18815 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18816 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18817 assertNull(calibrator.getEstimatedBiasFx());
18818 assertNull(calibrator.getEstimatedBiasFy());
18819 assertNull(calibrator.getEstimatedBiasFz());
18820 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18821 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18822 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18823 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18824 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18825 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18826 assertNull(calibrator.getEstimatedMa());
18827 assertNull(calibrator.getEstimatedSx());
18828 assertNull(calibrator.getEstimatedSy());
18829 assertNull(calibrator.getEstimatedSz());
18830 assertNull(calibrator.getEstimatedMxy());
18831 assertNull(calibrator.getEstimatedMxz());
18832 assertNull(calibrator.getEstimatedMyx());
18833 assertNull(calibrator.getEstimatedMyz());
18834 assertNull(calibrator.getEstimatedMzx());
18835 assertNull(calibrator.getEstimatedMzy());
18836 assertNull(calibrator.getEstimatedCovariance());
18837 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18838 assertNotNull(calibrator.getGroundTruthGravityNorm());
18839 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18840 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18841 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18842 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18843 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18844 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18845 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18846
18847
18848 calibrator = null;
18849 try {
18850 calibrator = new KnownGravityNormAccelerometerCalibrator(
18851 -gravityNorm, measurements,
18852 true, ba, this);
18853 fail("IllegalArgumentException expected but not thrown");
18854 } catch (final IllegalArgumentException ignore) {
18855 }
18856 try {
18857 calibrator = new KnownGravityNormAccelerometerCalibrator(
18858 gravityNorm, measurements, true,
18859 new Matrix(1, 1), this);
18860 fail("IllegalArgumentException expected but not thrown");
18861 } catch (final IllegalArgumentException ignore) {
18862 }
18863 try {
18864 calibrator = new KnownGravityNormAccelerometerCalibrator(
18865 gravityNorm, measurements, true,
18866 new Matrix(1, 3), this);
18867 fail("IllegalArgumentException expected but not thrown");
18868 } catch (final IllegalArgumentException ignore) {
18869 }
18870 assertNull(calibrator);
18871 }
18872
18873 @Test
18874 public void testConstructor149() throws WrongSizeException {
18875 final Matrix ba = generateBa();
18876 final double[] bias = ba.getBuffer();
18877 final double biasX = ba.getElementAtIndex(0);
18878 final double biasY = ba.getElementAtIndex(1);
18879 final double biasZ = ba.getElementAtIndex(2);
18880
18881 final Matrix ma = generateMaCommonAxis();
18882 final double sx = ma.getElementAt(0, 0);
18883 final double sy = ma.getElementAt(1, 1);
18884 final double sz = ma.getElementAt(2, 2);
18885 final double mxy = ma.getElementAt(0, 1);
18886 final double mxz = ma.getElementAt(0, 2);
18887 final double myx = ma.getElementAt(1, 0);
18888 final double myz = ma.getElementAt(1, 2);
18889 final double mzx = ma.getElementAt(2, 0);
18890 final double mzy = ma.getElementAt(2, 1);
18891
18892 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18893 final double latitude = Math.toRadians(
18894 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18895 final double longitude = Math.toRadians(
18896 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18897 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18898 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18899 final NEDVelocity nedVelocity = new NEDVelocity();
18900 final ECEFPosition ecefPosition = new ECEFPosition();
18901 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18902 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18903 ecefPosition, ecefVelocity);
18904 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18905 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18906 final double gravityNorm = gravity.getNorm();
18907
18908 KnownGravityNormAccelerometerCalibrator calibrator =
18909 new KnownGravityNormAccelerometerCalibrator(
18910 gravityNorm, ba, ma);
18911
18912
18913 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18914 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18915 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18916 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18917 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18918 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18919 final Acceleration bx2 = new Acceleration(0.0,
18920 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18921 calibrator.getInitialBiasXAsAcceleration(bx2);
18922 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18923 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18924 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18925 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18926 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18927 final Acceleration by2 = new Acceleration(0.0,
18928 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18929 calibrator.getInitialBiasYAsAcceleration(by2);
18930 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18931 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18932 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18933 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18934 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18935 final Acceleration bz2 = new Acceleration(0.0,
18936 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18937 calibrator.getInitialBiasZAsAcceleration(bz2);
18938 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18939 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18940 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18941 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18942 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18943 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18944 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18945 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18946 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18947 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18948 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18949 final double[] bias1 = calibrator.getInitialBias();
18950 assertArrayEquals(bias1, bias, 0.0);
18951 final double[] bias2 = new double[3];
18952 calibrator.getInitialBias(bias2);
18953 assertArrayEquals(bias1, bias2, 0.0);
18954 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18955 assertEquals(b1, ba);
18956 final Matrix b2 = new Matrix(3, 1);
18957 calibrator.getInitialBiasAsMatrix(b2);
18958 assertEquals(b1, b2);
18959 final Matrix ma1 = new Matrix(3, 3);
18960 ma1.setSubmatrix(0, 0,
18961 2, 2,
18962 new double[]{sx, myx, mzx,
18963 mxy, sy, mzy,
18964 mxz, myz, sz});
18965 assertEquals(calibrator.getInitialMa(), ma1);
18966 final Matrix ma2 = new Matrix(3, 3);
18967 calibrator.getInitialMa(ma2);
18968 assertEquals(ma1, ma2);
18969 assertNull(calibrator.getMeasurements());
18970 assertFalse(calibrator.isCommonAxisUsed());
18971 assertNull(calibrator.getListener());
18972 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18973 assertFalse(calibrator.isReady());
18974 assertFalse(calibrator.isRunning());
18975 assertNull(calibrator.getEstimatedBiases());
18976 assertFalse(calibrator.getEstimatedBiases(null));
18977 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18978 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18979 assertNull(calibrator.getEstimatedBiasFx());
18980 assertNull(calibrator.getEstimatedBiasFy());
18981 assertNull(calibrator.getEstimatedBiasFz());
18982 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18983 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18984 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18985 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18986 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18987 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18988 assertNull(calibrator.getEstimatedMa());
18989 assertNull(calibrator.getEstimatedSx());
18990 assertNull(calibrator.getEstimatedSy());
18991 assertNull(calibrator.getEstimatedSz());
18992 assertNull(calibrator.getEstimatedMxy());
18993 assertNull(calibrator.getEstimatedMxz());
18994 assertNull(calibrator.getEstimatedMyx());
18995 assertNull(calibrator.getEstimatedMyz());
18996 assertNull(calibrator.getEstimatedMzx());
18997 assertNull(calibrator.getEstimatedMzy());
18998 assertNull(calibrator.getEstimatedCovariance());
18999 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19000 assertNotNull(calibrator.getGroundTruthGravityNorm());
19001 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19002 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19003 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19004 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19005 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19006 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19007 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19008
19009
19010 calibrator = null;
19011 try {
19012 calibrator = new KnownGravityNormAccelerometerCalibrator(
19013 -gravityNorm, ba, ma);
19014 fail("IllegalArgumentException expected but not thrown");
19015 } catch (final IllegalArgumentException ignore) {
19016 }
19017 try {
19018 calibrator = new KnownGravityNormAccelerometerCalibrator(
19019 gravityNorm, new Matrix(1, 1), ma);
19020 fail("IllegalArgumentException expected but not thrown");
19021 } catch (final IllegalArgumentException ignore) {
19022 }
19023 try {
19024 calibrator = new KnownGravityNormAccelerometerCalibrator(
19025 gravityNorm, new Matrix(1, 3), ma);
19026 fail("IllegalArgumentException expected but not thrown");
19027 } catch (final IllegalArgumentException ignore) {
19028 }
19029 try {
19030 calibrator = new KnownGravityNormAccelerometerCalibrator(
19031 gravityNorm, ba, new Matrix(1, 3));
19032 fail("IllegalArgumentException expected but not thrown");
19033 } catch (final IllegalArgumentException ignore) {
19034 }
19035 try {
19036 calibrator = new KnownGravityNormAccelerometerCalibrator(
19037 gravityNorm, ba, new Matrix(3, 1));
19038 fail("IllegalArgumentException expected but not thrown");
19039 } catch (final IllegalArgumentException ignore) {
19040 }
19041 assertNull(calibrator);
19042 }
19043
19044 @Test
19045 public void testConstructor150() throws WrongSizeException {
19046 final Matrix ba = generateBa();
19047 final double[] bias = ba.getBuffer();
19048 final double biasX = ba.getElementAtIndex(0);
19049 final double biasY = ba.getElementAtIndex(1);
19050 final double biasZ = ba.getElementAtIndex(2);
19051
19052 final Matrix ma = generateMaCommonAxis();
19053 final double sx = ma.getElementAt(0, 0);
19054 final double sy = ma.getElementAt(1, 1);
19055 final double sz = ma.getElementAt(2, 2);
19056 final double mxy = ma.getElementAt(0, 1);
19057 final double mxz = ma.getElementAt(0, 2);
19058 final double myx = ma.getElementAt(1, 0);
19059 final double myz = ma.getElementAt(1, 2);
19060 final double mzx = ma.getElementAt(2, 0);
19061 final double mzy = ma.getElementAt(2, 1);
19062
19063 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19064 final double latitude = Math.toRadians(
19065 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19066 final double longitude = Math.toRadians(
19067 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19068 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19069 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19070 final NEDVelocity nedVelocity = new NEDVelocity();
19071 final ECEFPosition ecefPosition = new ECEFPosition();
19072 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19073 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19074 ecefPosition, ecefVelocity);
19075 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19076 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19077 final double gravityNorm = gravity.getNorm();
19078
19079 KnownGravityNormAccelerometerCalibrator calibrator =
19080 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19081 ba, ma, this);
19082
19083
19084 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19085 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19086 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19087 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19088 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19089 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19090 final Acceleration bx2 = new Acceleration(0.0,
19091 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19092 calibrator.getInitialBiasXAsAcceleration(bx2);
19093 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19094 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19095 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19096 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19097 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19098 final Acceleration by2 = new Acceleration(0.0,
19099 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19100 calibrator.getInitialBiasYAsAcceleration(by2);
19101 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19102 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19103 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19104 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19105 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19106 final Acceleration bz2 = new Acceleration(0.0,
19107 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19108 calibrator.getInitialBiasZAsAcceleration(bz2);
19109 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19110 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19111 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19112 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19113 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19114 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19115 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19116 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19117 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19118 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19119 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19120 final double[] bias1 = calibrator.getInitialBias();
19121 assertArrayEquals(bias1, bias, 0.0);
19122 final double[] bias2 = new double[3];
19123 calibrator.getInitialBias(bias2);
19124 assertArrayEquals(bias1, bias2, 0.0);
19125 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19126 assertEquals(b1, ba);
19127 final Matrix b2 = new Matrix(3, 1);
19128 calibrator.getInitialBiasAsMatrix(b2);
19129 assertEquals(b1, b2);
19130 final Matrix ma1 = new Matrix(3, 3);
19131 ma1.setSubmatrix(0, 0,
19132 2, 2,
19133 new double[]{sx, myx, mzx,
19134 mxy, sy, mzy,
19135 mxz, myz, sz});
19136 assertEquals(calibrator.getInitialMa(), ma1);
19137 final Matrix ma2 = new Matrix(3, 3);
19138 calibrator.getInitialMa(ma2);
19139 assertEquals(ma1, ma2);
19140 assertNull(calibrator.getMeasurements());
19141 assertFalse(calibrator.isCommonAxisUsed());
19142 assertSame(calibrator.getListener(), this);
19143 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19144 assertFalse(calibrator.isReady());
19145 assertFalse(calibrator.isRunning());
19146 assertNull(calibrator.getEstimatedBiases());
19147 assertFalse(calibrator.getEstimatedBiases(null));
19148 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19149 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19150 assertNull(calibrator.getEstimatedBiasFx());
19151 assertNull(calibrator.getEstimatedBiasFy());
19152 assertNull(calibrator.getEstimatedBiasFz());
19153 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19154 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19155 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19156 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19157 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19158 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19159 assertNull(calibrator.getEstimatedMa());
19160 assertNull(calibrator.getEstimatedSx());
19161 assertNull(calibrator.getEstimatedSy());
19162 assertNull(calibrator.getEstimatedSz());
19163 assertNull(calibrator.getEstimatedMxy());
19164 assertNull(calibrator.getEstimatedMxz());
19165 assertNull(calibrator.getEstimatedMyx());
19166 assertNull(calibrator.getEstimatedMyz());
19167 assertNull(calibrator.getEstimatedMzx());
19168 assertNull(calibrator.getEstimatedMzy());
19169 assertNull(calibrator.getEstimatedCovariance());
19170 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19171 assertNotNull(calibrator.getGroundTruthGravityNorm());
19172 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19173 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19174 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19175 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19176 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19177 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19178 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19179
19180
19181 calibrator = null;
19182 try {
19183 calibrator = new KnownGravityNormAccelerometerCalibrator(
19184 -gravityNorm, ba, ma, this);
19185 fail("IllegalArgumentException expected but not thrown");
19186 } catch (final IllegalArgumentException ignore) {
19187 }
19188 try {
19189 calibrator = new KnownGravityNormAccelerometerCalibrator(
19190 gravityNorm, new Matrix(1, 1), ma,
19191 this);
19192 fail("IllegalArgumentException expected but not thrown");
19193 } catch (final IllegalArgumentException ignore) {
19194 }
19195 try {
19196 calibrator = new KnownGravityNormAccelerometerCalibrator(
19197 gravityNorm, new Matrix(1, 3), ma,
19198 this);
19199 fail("IllegalArgumentException expected but not thrown");
19200 } catch (final IllegalArgumentException ignore) {
19201 }
19202 try {
19203 calibrator = new KnownGravityNormAccelerometerCalibrator(
19204 gravityNorm, ba, new Matrix(1, 3), this);
19205 fail("IllegalArgumentException expected but not thrown");
19206 } catch (final IllegalArgumentException ignore) {
19207 }
19208 try {
19209 calibrator = new KnownGravityNormAccelerometerCalibrator(
19210 gravityNorm, ba, new Matrix(3, 1), this);
19211 fail("IllegalArgumentException expected but not thrown");
19212 } catch (final IllegalArgumentException ignore) {
19213 }
19214 assertNull(calibrator);
19215 }
19216
19217 @Test
19218 public void testConstructor151() throws WrongSizeException {
19219 final Collection<StandardDeviationBodyKinematics> measurements =
19220 Collections.emptyList();
19221
19222 final Matrix ba = generateBa();
19223 final double[] bias = ba.getBuffer();
19224 final double biasX = ba.getElementAtIndex(0);
19225 final double biasY = ba.getElementAtIndex(1);
19226 final double biasZ = ba.getElementAtIndex(2);
19227
19228 final Matrix ma = generateMaCommonAxis();
19229 final double sx = ma.getElementAt(0, 0);
19230 final double sy = ma.getElementAt(1, 1);
19231 final double sz = ma.getElementAt(2, 2);
19232 final double mxy = ma.getElementAt(0, 1);
19233 final double mxz = ma.getElementAt(0, 2);
19234 final double myx = ma.getElementAt(1, 0);
19235 final double myz = ma.getElementAt(1, 2);
19236 final double mzx = ma.getElementAt(2, 0);
19237 final double mzy = ma.getElementAt(2, 1);
19238
19239 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19240 final double latitude = Math.toRadians(
19241 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19242 final double longitude = Math.toRadians(
19243 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19244 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19245 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19246 final NEDVelocity nedVelocity = new NEDVelocity();
19247 final ECEFPosition ecefPosition = new ECEFPosition();
19248 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19249 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19250 ecefPosition, ecefVelocity);
19251 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19252 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19253 final double gravityNorm = gravity.getNorm();
19254
19255 KnownGravityNormAccelerometerCalibrator calibrator =
19256 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19257 measurements, ba, ma);
19258
19259
19260 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19261 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19262 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19263 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19264 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19265 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19266 final Acceleration bx2 = new Acceleration(0.0,
19267 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19268 calibrator.getInitialBiasXAsAcceleration(bx2);
19269 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19270 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19271 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19272 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19273 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19274 final Acceleration by2 = new Acceleration(0.0,
19275 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19276 calibrator.getInitialBiasYAsAcceleration(by2);
19277 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19278 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19279 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19280 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19281 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19282 final Acceleration bz2 = new Acceleration(0.0,
19283 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19284 calibrator.getInitialBiasZAsAcceleration(bz2);
19285 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19286 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19287 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19288 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19289 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19290 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19291 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19292 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19293 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19294 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19295 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19296 final double[] bias1 = calibrator.getInitialBias();
19297 assertArrayEquals(bias1, bias, 0.0);
19298 final double[] bias2 = new double[3];
19299 calibrator.getInitialBias(bias2);
19300 assertArrayEquals(bias1, bias2, 0.0);
19301 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19302 assertEquals(b1, ba);
19303 final Matrix b2 = new Matrix(3, 1);
19304 calibrator.getInitialBiasAsMatrix(b2);
19305 assertEquals(b1, b2);
19306 final Matrix ma1 = new Matrix(3, 3);
19307 ma1.setSubmatrix(0, 0,
19308 2, 2,
19309 new double[]{sx, myx, mzx,
19310 mxy, sy, mzy,
19311 mxz, myz, sz});
19312 assertEquals(calibrator.getInitialMa(), ma1);
19313 final Matrix ma2 = new Matrix(3, 3);
19314 calibrator.getInitialMa(ma2);
19315 assertEquals(ma1, ma2);
19316 assertSame(calibrator.getMeasurements(), measurements);
19317 assertFalse(calibrator.isCommonAxisUsed());
19318 assertNull(calibrator.getListener());
19319 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19320 assertFalse(calibrator.isReady());
19321 assertFalse(calibrator.isRunning());
19322 assertNull(calibrator.getEstimatedBiases());
19323 assertFalse(calibrator.getEstimatedBiases(null));
19324 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19325 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19326 assertNull(calibrator.getEstimatedBiasFx());
19327 assertNull(calibrator.getEstimatedBiasFy());
19328 assertNull(calibrator.getEstimatedBiasFz());
19329 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19330 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19331 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19332 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19333 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19334 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19335 assertNull(calibrator.getEstimatedMa());
19336 assertNull(calibrator.getEstimatedSx());
19337 assertNull(calibrator.getEstimatedSy());
19338 assertNull(calibrator.getEstimatedSz());
19339 assertNull(calibrator.getEstimatedMxy());
19340 assertNull(calibrator.getEstimatedMxz());
19341 assertNull(calibrator.getEstimatedMyx());
19342 assertNull(calibrator.getEstimatedMyz());
19343 assertNull(calibrator.getEstimatedMzx());
19344 assertNull(calibrator.getEstimatedMzy());
19345 assertNull(calibrator.getEstimatedCovariance());
19346 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19347 assertNotNull(calibrator.getGroundTruthGravityNorm());
19348 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19349 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19350 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19351 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19352 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19353 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19354 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19355
19356
19357 calibrator = null;
19358 try {
19359 calibrator = new KnownGravityNormAccelerometerCalibrator(
19360 -gravityNorm, measurements, ba, ma);
19361 fail("IllegalArgumentException expected but not thrown");
19362 } catch (final IllegalArgumentException ignore) {
19363 }
19364 try {
19365 calibrator = new KnownGravityNormAccelerometerCalibrator(
19366 gravityNorm, measurements,
19367 new Matrix(1, 1), ma);
19368 fail("IllegalArgumentException expected but not thrown");
19369 } catch (final IllegalArgumentException ignore) {
19370 }
19371 try {
19372 calibrator = new KnownGravityNormAccelerometerCalibrator(
19373 gravityNorm, measurements,
19374 new Matrix(1, 3), ma);
19375 fail("IllegalArgumentException expected but not thrown");
19376 } catch (final IllegalArgumentException ignore) {
19377 }
19378 try {
19379 calibrator = new KnownGravityNormAccelerometerCalibrator(
19380 gravityNorm, measurements, ba,
19381 new Matrix(1, 3));
19382 fail("IllegalArgumentException expected but not thrown");
19383 } catch (final IllegalArgumentException ignore) {
19384 }
19385 try {
19386 calibrator = new KnownGravityNormAccelerometerCalibrator(
19387 gravityNorm, measurements, ba,
19388 new Matrix(3, 1));
19389 fail("IllegalArgumentException expected but not thrown");
19390 } catch (final IllegalArgumentException ignore) {
19391 }
19392 assertNull(calibrator);
19393 }
19394
19395 @Test
19396 public void testConstructor152() throws WrongSizeException {
19397 final Collection<StandardDeviationBodyKinematics> measurements =
19398 Collections.emptyList();
19399
19400 final Matrix ba = generateBa();
19401 final double[] bias = ba.getBuffer();
19402 final double biasX = ba.getElementAtIndex(0);
19403 final double biasY = ba.getElementAtIndex(1);
19404 final double biasZ = ba.getElementAtIndex(2);
19405
19406 final Matrix ma = generateMaCommonAxis();
19407 final double sx = ma.getElementAt(0, 0);
19408 final double sy = ma.getElementAt(1, 1);
19409 final double sz = ma.getElementAt(2, 2);
19410 final double mxy = ma.getElementAt(0, 1);
19411 final double mxz = ma.getElementAt(0, 2);
19412 final double myx = ma.getElementAt(1, 0);
19413 final double myz = ma.getElementAt(1, 2);
19414 final double mzx = ma.getElementAt(2, 0);
19415 final double mzy = ma.getElementAt(2, 1);
19416
19417 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19418 final double latitude = Math.toRadians(
19419 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19420 final double longitude = Math.toRadians(
19421 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19422 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19423 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19424 final NEDVelocity nedVelocity = new NEDVelocity();
19425 final ECEFPosition ecefPosition = new ECEFPosition();
19426 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19427 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19428 ecefPosition, ecefVelocity);
19429 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19430 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19431 final double gravityNorm = gravity.getNorm();
19432
19433 KnownGravityNormAccelerometerCalibrator calibrator =
19434 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19435 measurements, ba, ma, this);
19436
19437
19438 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19439 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19440 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19441 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19442 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19443 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19444 final Acceleration bx2 = new Acceleration(0.0,
19445 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19446 calibrator.getInitialBiasXAsAcceleration(bx2);
19447 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19448 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19449 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19450 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19451 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19452 final Acceleration by2 = new Acceleration(0.0,
19453 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19454 calibrator.getInitialBiasYAsAcceleration(by2);
19455 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19456 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19457 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19458 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19459 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19460 final Acceleration bz2 = new Acceleration(0.0,
19461 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19462 calibrator.getInitialBiasZAsAcceleration(bz2);
19463 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19464 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19465 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19466 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19467 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19468 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19469 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19470 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19471 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19472 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19473 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19474 final double[] bias1 = calibrator.getInitialBias();
19475 assertArrayEquals(bias1, bias, 0.0);
19476 final double[] bias2 = new double[3];
19477 calibrator.getInitialBias(bias2);
19478 assertArrayEquals(bias1, bias2, 0.0);
19479 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19480 assertEquals(b1, ba);
19481 final Matrix b2 = new Matrix(3, 1);
19482 calibrator.getInitialBiasAsMatrix(b2);
19483 assertEquals(b1, b2);
19484 final Matrix ma1 = new Matrix(3, 3);
19485 ma1.setSubmatrix(0, 0,
19486 2, 2,
19487 new double[]{sx, myx, mzx,
19488 mxy, sy, mzy,
19489 mxz, myz, sz});
19490 assertEquals(calibrator.getInitialMa(), ma1);
19491 final Matrix ma2 = new Matrix(3, 3);
19492 calibrator.getInitialMa(ma2);
19493 assertEquals(ma1, ma2);
19494 assertSame(calibrator.getMeasurements(), measurements);
19495 assertFalse(calibrator.isCommonAxisUsed());
19496 assertSame(calibrator.getListener(), this);
19497 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19498 assertFalse(calibrator.isReady());
19499 assertFalse(calibrator.isRunning());
19500 assertNull(calibrator.getEstimatedBiases());
19501 assertFalse(calibrator.getEstimatedBiases(null));
19502 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19503 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19504 assertNull(calibrator.getEstimatedBiasFx());
19505 assertNull(calibrator.getEstimatedBiasFy());
19506 assertNull(calibrator.getEstimatedBiasFz());
19507 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19508 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19509 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19510 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19511 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19512 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19513 assertNull(calibrator.getEstimatedMa());
19514 assertNull(calibrator.getEstimatedSx());
19515 assertNull(calibrator.getEstimatedSy());
19516 assertNull(calibrator.getEstimatedSz());
19517 assertNull(calibrator.getEstimatedMxy());
19518 assertNull(calibrator.getEstimatedMxz());
19519 assertNull(calibrator.getEstimatedMyx());
19520 assertNull(calibrator.getEstimatedMyz());
19521 assertNull(calibrator.getEstimatedMzx());
19522 assertNull(calibrator.getEstimatedMzy());
19523 assertNull(calibrator.getEstimatedCovariance());
19524 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19525 assertNotNull(calibrator.getGroundTruthGravityNorm());
19526 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19527 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19528 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19529 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19530 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19531 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19532 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19533
19534
19535 calibrator = null;
19536 try {
19537 calibrator = new KnownGravityNormAccelerometerCalibrator(
19538 -gravityNorm, measurements, ba, ma, this);
19539 fail("IllegalArgumentException expected but not thrown");
19540 } catch (final IllegalArgumentException ignore) {
19541 }
19542 try {
19543 calibrator = new KnownGravityNormAccelerometerCalibrator(
19544 gravityNorm, measurements,
19545 new Matrix(1, 1), ma, this);
19546 fail("IllegalArgumentException expected but not thrown");
19547 } catch (final IllegalArgumentException ignore) {
19548 }
19549 try {
19550 calibrator = new KnownGravityNormAccelerometerCalibrator(
19551 gravityNorm, measurements,
19552 new Matrix(1, 3), ma, this);
19553 fail("IllegalArgumentException expected but not thrown");
19554 } catch (final IllegalArgumentException ignore) {
19555 }
19556 try {
19557 calibrator = new KnownGravityNormAccelerometerCalibrator(
19558 gravityNorm, measurements, ba,
19559 new Matrix(1, 3), this);
19560 fail("IllegalArgumentException expected but not thrown");
19561 } catch (final IllegalArgumentException ignore) {
19562 }
19563 try {
19564 calibrator = new KnownGravityNormAccelerometerCalibrator(
19565 gravityNorm, measurements, ba,
19566 new Matrix(3, 1), this);
19567 fail("IllegalArgumentException expected but not thrown");
19568 } catch (final IllegalArgumentException ignore) {
19569 }
19570 assertNull(calibrator);
19571 }
19572
19573 @Test
19574 public void testConstructor153() throws WrongSizeException {
19575 final Matrix ba = generateBa();
19576 final double[] bias = ba.getBuffer();
19577 final double biasX = ba.getElementAtIndex(0);
19578 final double biasY = ba.getElementAtIndex(1);
19579 final double biasZ = ba.getElementAtIndex(2);
19580
19581 final Matrix ma = generateMaCommonAxis();
19582 final double sx = ma.getElementAt(0, 0);
19583 final double sy = ma.getElementAt(1, 1);
19584 final double sz = ma.getElementAt(2, 2);
19585 final double mxy = ma.getElementAt(0, 1);
19586 final double mxz = ma.getElementAt(0, 2);
19587 final double myx = ma.getElementAt(1, 0);
19588 final double myz = ma.getElementAt(1, 2);
19589 final double mzx = ma.getElementAt(2, 0);
19590 final double mzy = ma.getElementAt(2, 1);
19591
19592 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19593 final double latitude = Math.toRadians(
19594 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19595 final double longitude = Math.toRadians(
19596 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19597 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19598 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19599 final NEDVelocity nedVelocity = new NEDVelocity();
19600 final ECEFPosition ecefPosition = new ECEFPosition();
19601 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19602 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19603 ecefPosition, ecefVelocity);
19604 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19605 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19606 final double gravityNorm = gravity.getNorm();
19607
19608 KnownGravityNormAccelerometerCalibrator calibrator =
19609 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19610 true, ba, ma);
19611
19612
19613 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19614 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19615 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19616 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19617 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19618 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19619 final Acceleration bx2 = new Acceleration(0.0,
19620 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19621 calibrator.getInitialBiasXAsAcceleration(bx2);
19622 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19623 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19624 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19625 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19626 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19627 final Acceleration by2 = new Acceleration(0.0,
19628 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19629 calibrator.getInitialBiasYAsAcceleration(by2);
19630 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19631 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19632 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19633 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19634 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19635 final Acceleration bz2 = new Acceleration(0.0,
19636 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19637 calibrator.getInitialBiasZAsAcceleration(bz2);
19638 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19639 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19640 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19641 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19642 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19643 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19644 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19645 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19646 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19647 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19648 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19649 final double[] bias1 = calibrator.getInitialBias();
19650 assertArrayEquals(bias1, bias, 0.0);
19651 final double[] bias2 = new double[3];
19652 calibrator.getInitialBias(bias2);
19653 assertArrayEquals(bias1, bias2, 0.0);
19654 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19655 assertEquals(b1, ba);
19656 final Matrix b2 = new Matrix(3, 1);
19657 calibrator.getInitialBiasAsMatrix(b2);
19658 assertEquals(b1, b2);
19659 final Matrix ma1 = new Matrix(3, 3);
19660 ma1.setSubmatrix(0, 0,
19661 2, 2,
19662 new double[]{sx, myx, mzx,
19663 mxy, sy, mzy,
19664 mxz, myz, sz});
19665 assertEquals(calibrator.getInitialMa(), ma1);
19666 final Matrix ma2 = new Matrix(3, 3);
19667 calibrator.getInitialMa(ma2);
19668 assertEquals(ma1, ma2);
19669 assertNull(calibrator.getMeasurements());
19670 assertTrue(calibrator.isCommonAxisUsed());
19671 assertNull(calibrator.getListener());
19672 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19673 assertFalse(calibrator.isReady());
19674 assertFalse(calibrator.isRunning());
19675 assertNull(calibrator.getEstimatedBiases());
19676 assertFalse(calibrator.getEstimatedBiases(null));
19677 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19678 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19679 assertNull(calibrator.getEstimatedBiasFx());
19680 assertNull(calibrator.getEstimatedBiasFy());
19681 assertNull(calibrator.getEstimatedBiasFz());
19682 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19683 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19684 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19685 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19686 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19687 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19688 assertNull(calibrator.getEstimatedMa());
19689 assertNull(calibrator.getEstimatedSx());
19690 assertNull(calibrator.getEstimatedSy());
19691 assertNull(calibrator.getEstimatedSz());
19692 assertNull(calibrator.getEstimatedMxy());
19693 assertNull(calibrator.getEstimatedMxz());
19694 assertNull(calibrator.getEstimatedMyx());
19695 assertNull(calibrator.getEstimatedMyz());
19696 assertNull(calibrator.getEstimatedMzx());
19697 assertNull(calibrator.getEstimatedMzy());
19698 assertNull(calibrator.getEstimatedCovariance());
19699 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19700 assertNotNull(calibrator.getGroundTruthGravityNorm());
19701 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19702 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19703 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19704 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19705 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19706 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19707 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19708
19709
19710 calibrator = null;
19711 try {
19712 calibrator = new KnownGravityNormAccelerometerCalibrator(
19713 -gravityNorm, true, ba, ma);
19714 fail("IllegalArgumentException expected but not thrown");
19715 } catch (final IllegalArgumentException ignore) {
19716 }
19717 try {
19718 calibrator = new KnownGravityNormAccelerometerCalibrator(
19719 gravityNorm, true,
19720 new Matrix(1, 1), ma);
19721 fail("IllegalArgumentException expected but not thrown");
19722 } catch (final IllegalArgumentException ignore) {
19723 }
19724 try {
19725 calibrator = new KnownGravityNormAccelerometerCalibrator(
19726 gravityNorm, true,
19727 new Matrix(1, 3), ma);
19728 fail("IllegalArgumentException expected but not thrown");
19729 } catch (final IllegalArgumentException ignore) {
19730 }
19731 try {
19732 calibrator = new KnownGravityNormAccelerometerCalibrator(
19733 gravityNorm, true, ba,
19734 new Matrix(1, 3));
19735 fail("IllegalArgumentException expected but not thrown");
19736 } catch (final IllegalArgumentException ignore) {
19737 }
19738 try {
19739 calibrator = new KnownGravityNormAccelerometerCalibrator(
19740 gravityNorm, true, ba,
19741 new Matrix(3, 1));
19742 fail("IllegalArgumentException expected but not thrown");
19743 } catch (final IllegalArgumentException ignore) {
19744 }
19745 assertNull(calibrator);
19746 }
19747
19748 @Test
19749 public void testConstructor154() throws WrongSizeException {
19750 final Matrix ba = generateBa();
19751 final double[] bias = ba.getBuffer();
19752 final double biasX = ba.getElementAtIndex(0);
19753 final double biasY = ba.getElementAtIndex(1);
19754 final double biasZ = ba.getElementAtIndex(2);
19755
19756 final Matrix ma = generateMaCommonAxis();
19757 final double sx = ma.getElementAt(0, 0);
19758 final double sy = ma.getElementAt(1, 1);
19759 final double sz = ma.getElementAt(2, 2);
19760 final double mxy = ma.getElementAt(0, 1);
19761 final double mxz = ma.getElementAt(0, 2);
19762 final double myx = ma.getElementAt(1, 0);
19763 final double myz = ma.getElementAt(1, 2);
19764 final double mzx = ma.getElementAt(2, 0);
19765 final double mzy = ma.getElementAt(2, 1);
19766
19767 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19768 final double latitude = Math.toRadians(
19769 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19770 final double longitude = Math.toRadians(
19771 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19772 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19773 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19774 final NEDVelocity nedVelocity = new NEDVelocity();
19775 final ECEFPosition ecefPosition = new ECEFPosition();
19776 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19777 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19778 ecefPosition, ecefVelocity);
19779 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19780 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19781 final double gravityNorm = gravity.getNorm();
19782
19783 KnownGravityNormAccelerometerCalibrator calibrator =
19784 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19785 true, ba, ma, this);
19786
19787
19788 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19789 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19790 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19791 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19792 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19793 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19794 final Acceleration bx2 = new Acceleration(0.0,
19795 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19796 calibrator.getInitialBiasXAsAcceleration(bx2);
19797 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19798 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19799 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19800 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19801 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19802 final Acceleration by2 = new Acceleration(0.0,
19803 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19804 calibrator.getInitialBiasYAsAcceleration(by2);
19805 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19806 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19807 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19808 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19809 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19810 final Acceleration bz2 = new Acceleration(0.0,
19811 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19812 calibrator.getInitialBiasZAsAcceleration(bz2);
19813 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19814 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19815 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19816 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19817 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19818 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19819 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19820 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19821 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19822 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19823 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19824 final double[] bias1 = calibrator.getInitialBias();
19825 assertArrayEquals(bias1, bias, 0.0);
19826 final double[] bias2 = new double[3];
19827 calibrator.getInitialBias(bias2);
19828 assertArrayEquals(bias1, bias2, 0.0);
19829 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19830 assertEquals(b1, ba);
19831 final Matrix b2 = new Matrix(3, 1);
19832 calibrator.getInitialBiasAsMatrix(b2);
19833 assertEquals(b1, b2);
19834 final Matrix ma1 = new Matrix(3, 3);
19835 ma1.setSubmatrix(0, 0,
19836 2, 2,
19837 new double[]{sx, myx, mzx,
19838 mxy, sy, mzy,
19839 mxz, myz, sz});
19840 assertEquals(calibrator.getInitialMa(), ma1);
19841 final Matrix ma2 = new Matrix(3, 3);
19842 calibrator.getInitialMa(ma2);
19843 assertEquals(ma1, ma2);
19844 assertNull(calibrator.getMeasurements());
19845 assertTrue(calibrator.isCommonAxisUsed());
19846 assertSame(calibrator.getListener(), this);
19847 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19848 assertFalse(calibrator.isReady());
19849 assertFalse(calibrator.isRunning());
19850 assertNull(calibrator.getEstimatedBiases());
19851 assertFalse(calibrator.getEstimatedBiases(null));
19852 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19853 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19854 assertNull(calibrator.getEstimatedBiasFx());
19855 assertNull(calibrator.getEstimatedBiasFy());
19856 assertNull(calibrator.getEstimatedBiasFz());
19857 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19858 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19859 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19860 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19861 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19862 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19863 assertNull(calibrator.getEstimatedMa());
19864 assertNull(calibrator.getEstimatedSx());
19865 assertNull(calibrator.getEstimatedSy());
19866 assertNull(calibrator.getEstimatedSz());
19867 assertNull(calibrator.getEstimatedMxy());
19868 assertNull(calibrator.getEstimatedMxz());
19869 assertNull(calibrator.getEstimatedMyx());
19870 assertNull(calibrator.getEstimatedMyz());
19871 assertNull(calibrator.getEstimatedMzx());
19872 assertNull(calibrator.getEstimatedMzy());
19873 assertNull(calibrator.getEstimatedCovariance());
19874 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19875 assertNotNull(calibrator.getGroundTruthGravityNorm());
19876 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19877 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19878 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19879 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19880 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19881 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19882 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19883
19884
19885 calibrator = null;
19886 try {
19887 calibrator = new KnownGravityNormAccelerometerCalibrator(
19888 -gravityNorm, true, ba, ma, this);
19889 fail("IllegalArgumentException expected but not thrown");
19890 } catch (final IllegalArgumentException ignore) {
19891 }
19892 try {
19893 calibrator = new KnownGravityNormAccelerometerCalibrator(
19894 gravityNorm, true, new Matrix(1, 1),
19895 ma, this);
19896 fail("IllegalArgumentException expected but not thrown");
19897 } catch (final IllegalArgumentException ignore) {
19898 }
19899 try {
19900 calibrator = new KnownGravityNormAccelerometerCalibrator(
19901 gravityNorm, true, new Matrix(1, 3),
19902 ma, this);
19903 fail("IllegalArgumentException expected but not thrown");
19904 } catch (final IllegalArgumentException ignore) {
19905 }
19906 try {
19907 calibrator = new KnownGravityNormAccelerometerCalibrator(
19908 gravityNorm, true, ba, new Matrix(1, 3),
19909 this);
19910 fail("IllegalArgumentException expected but not thrown");
19911 } catch (final IllegalArgumentException ignore) {
19912 }
19913 try {
19914 calibrator = new KnownGravityNormAccelerometerCalibrator(
19915 gravityNorm, true, ba, new Matrix(3, 1),
19916 this);
19917 fail("IllegalArgumentException expected but not thrown");
19918 } catch (final IllegalArgumentException ignore) {
19919 }
19920 assertNull(calibrator);
19921 }
19922
19923 @Test
19924 public void testConstructor155() throws WrongSizeException {
19925 final Collection<StandardDeviationBodyKinematics> measurements =
19926 Collections.emptyList();
19927
19928 final Matrix ba = generateBa();
19929 final double[] bias = ba.getBuffer();
19930 final double biasX = ba.getElementAtIndex(0);
19931 final double biasY = ba.getElementAtIndex(1);
19932 final double biasZ = ba.getElementAtIndex(2);
19933
19934 final Matrix ma = generateMaCommonAxis();
19935 final double sx = ma.getElementAt(0, 0);
19936 final double sy = ma.getElementAt(1, 1);
19937 final double sz = ma.getElementAt(2, 2);
19938 final double mxy = ma.getElementAt(0, 1);
19939 final double mxz = ma.getElementAt(0, 2);
19940 final double myx = ma.getElementAt(1, 0);
19941 final double myz = ma.getElementAt(1, 2);
19942 final double mzx = ma.getElementAt(2, 0);
19943 final double mzy = ma.getElementAt(2, 1);
19944
19945 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19946 final double latitude = Math.toRadians(
19947 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19948 final double longitude = Math.toRadians(
19949 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19950 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19951 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19952 final NEDVelocity nedVelocity = new NEDVelocity();
19953 final ECEFPosition ecefPosition = new ECEFPosition();
19954 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19955 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19956 ecefPosition, ecefVelocity);
19957 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19958 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19959 final double gravityNorm = gravity.getNorm();
19960
19961 KnownGravityNormAccelerometerCalibrator calibrator =
19962 new KnownGravityNormAccelerometerCalibrator(
19963 gravityNorm, measurements,
19964 true, ba, ma);
19965
19966
19967 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19968 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19969 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19970 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19971 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19972 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19973 final Acceleration bx2 = new Acceleration(0.0,
19974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19975 calibrator.getInitialBiasXAsAcceleration(bx2);
19976 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19977 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19978 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19979 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19980 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19981 final Acceleration by2 = new Acceleration(0.0,
19982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19983 calibrator.getInitialBiasYAsAcceleration(by2);
19984 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19985 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19986 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19987 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19988 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19989 final Acceleration bz2 = new Acceleration(0.0,
19990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19991 calibrator.getInitialBiasZAsAcceleration(bz2);
19992 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19993 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19994 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19995 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19996 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19997 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19998 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19999 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
20000 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
20001 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
20002 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
20003 final double[] bias1 = calibrator.getInitialBias();
20004 assertArrayEquals(bias1, bias, 0.0);
20005 final double[] bias2 = new double[3];
20006 calibrator.getInitialBias(bias2);
20007 assertArrayEquals(bias1, bias2, 0.0);
20008 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20009 assertEquals(b1, ba);
20010 final Matrix b2 = new Matrix(3, 1);
20011 calibrator.getInitialBiasAsMatrix(b2);
20012 assertEquals(b1, b2);
20013 final Matrix ma1 = new Matrix(3, 3);
20014 ma1.setSubmatrix(0, 0,
20015 2, 2,
20016 new double[]{sx, myx, mzx,
20017 mxy, sy, mzy,
20018 mxz, myz, sz});
20019 assertEquals(calibrator.getInitialMa(), ma1);
20020 final Matrix ma2 = new Matrix(3, 3);
20021 calibrator.getInitialMa(ma2);
20022 assertEquals(ma1, ma2);
20023 assertSame(calibrator.getMeasurements(), measurements);
20024 assertTrue(calibrator.isCommonAxisUsed());
20025 assertNull(calibrator.getListener());
20026 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20027 assertFalse(calibrator.isReady());
20028 assertFalse(calibrator.isRunning());
20029 assertNull(calibrator.getEstimatedBiases());
20030 assertFalse(calibrator.getEstimatedBiases(null));
20031 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20032 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20033 assertNull(calibrator.getEstimatedBiasFx());
20034 assertNull(calibrator.getEstimatedBiasFy());
20035 assertNull(calibrator.getEstimatedBiasFz());
20036 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20037 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20038 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20039 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20040 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20041 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20042 assertNull(calibrator.getEstimatedMa());
20043 assertNull(calibrator.getEstimatedSx());
20044 assertNull(calibrator.getEstimatedSy());
20045 assertNull(calibrator.getEstimatedSz());
20046 assertNull(calibrator.getEstimatedMxy());
20047 assertNull(calibrator.getEstimatedMxz());
20048 assertNull(calibrator.getEstimatedMyx());
20049 assertNull(calibrator.getEstimatedMyz());
20050 assertNull(calibrator.getEstimatedMzx());
20051 assertNull(calibrator.getEstimatedMzy());
20052 assertNull(calibrator.getEstimatedCovariance());
20053 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20054 assertNotNull(calibrator.getGroundTruthGravityNorm());
20055 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20056 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20057 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20058 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20059 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20060 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20061 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20062
20063
20064 calibrator = null;
20065 try {
20066 calibrator = new KnownGravityNormAccelerometerCalibrator(
20067 -gravityNorm, measurements,
20068 true, ba, ma);
20069 fail("IllegalArgumentException expected but not thrown");
20070 } catch (final IllegalArgumentException ignore) {
20071 }
20072 try {
20073 calibrator = new KnownGravityNormAccelerometerCalibrator(
20074 gravityNorm, measurements, true,
20075 new Matrix(1, 1), ma);
20076 fail("IllegalArgumentException expected but not thrown");
20077 } catch (final IllegalArgumentException ignore) {
20078 }
20079 try {
20080 calibrator = new KnownGravityNormAccelerometerCalibrator(
20081 gravityNorm, measurements, true,
20082 new Matrix(1, 3), ma);
20083 fail("IllegalArgumentException expected but not thrown");
20084 } catch (final IllegalArgumentException ignore) {
20085 }
20086 try {
20087 calibrator = new KnownGravityNormAccelerometerCalibrator(
20088 gravityNorm, measurements, true,
20089 ba, new Matrix(1, 3));
20090 fail("IllegalArgumentException expected but not thrown");
20091 } catch (final IllegalArgumentException ignore) {
20092 }
20093 try {
20094 calibrator = new KnownGravityNormAccelerometerCalibrator(
20095 gravityNorm, measurements, true,
20096 ba, new Matrix(3, 1));
20097 fail("IllegalArgumentException expected but not thrown");
20098 } catch (final IllegalArgumentException ignore) {
20099 }
20100 assertNull(calibrator);
20101 }
20102
20103 @Test
20104 public void testConstructor156() throws WrongSizeException {
20105 final Collection<StandardDeviationBodyKinematics> measurements =
20106 Collections.emptyList();
20107
20108 final Matrix ba = generateBa();
20109 final double[] bias = ba.getBuffer();
20110 final double biasX = ba.getElementAtIndex(0);
20111 final double biasY = ba.getElementAtIndex(1);
20112 final double biasZ = ba.getElementAtIndex(2);
20113
20114 final Matrix ma = generateMaCommonAxis();
20115 final double sx = ma.getElementAt(0, 0);
20116 final double sy = ma.getElementAt(1, 1);
20117 final double sz = ma.getElementAt(2, 2);
20118 final double mxy = ma.getElementAt(0, 1);
20119 final double mxz = ma.getElementAt(0, 2);
20120 final double myx = ma.getElementAt(1, 0);
20121 final double myz = ma.getElementAt(1, 2);
20122 final double mzx = ma.getElementAt(2, 0);
20123 final double mzy = ma.getElementAt(2, 1);
20124
20125 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20126 final double latitude = Math.toRadians(
20127 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20128 final double longitude = Math.toRadians(
20129 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20130 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20131 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20132 final NEDVelocity nedVelocity = new NEDVelocity();
20133 final ECEFPosition ecefPosition = new ECEFPosition();
20134 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20135 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20136 ecefPosition, ecefVelocity);
20137 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20138 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20139 final double gravityNorm = gravity.getNorm();
20140
20141 KnownGravityNormAccelerometerCalibrator calibrator =
20142 new KnownGravityNormAccelerometerCalibrator(
20143 gravityNorm, measurements,
20144 true, ba, ma, this);
20145
20146
20147 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20148 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20149 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20150 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20151 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20152 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20153 final Acceleration bx2 = new Acceleration(0.0,
20154 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20155 calibrator.getInitialBiasXAsAcceleration(bx2);
20156 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20157 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20158 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20159 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20160 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20161 final Acceleration by2 = new Acceleration(0.0,
20162 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20163 calibrator.getInitialBiasYAsAcceleration(by2);
20164 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20165 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20166 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20167 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20168 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20169 final Acceleration bz2 = new Acceleration(0.0,
20170 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20171 calibrator.getInitialBiasZAsAcceleration(bz2);
20172 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20173 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20174 assertEquals(calibrator.getInitialSx(), sx, 0.0);
20175 assertEquals(calibrator.getInitialSy(), sy, 0.0);
20176 assertEquals(calibrator.getInitialSz(), sz, 0.0);
20177 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
20178 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
20179 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
20180 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
20181 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
20182 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
20183 final double[] bias1 = calibrator.getInitialBias();
20184 assertArrayEquals(bias1, bias, 0.0);
20185 final double[] bias2 = new double[3];
20186 calibrator.getInitialBias(bias2);
20187 assertArrayEquals(bias1, bias2, 0.0);
20188 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20189 assertEquals(b1, ba);
20190 final Matrix b2 = new Matrix(3, 1);
20191 calibrator.getInitialBiasAsMatrix(b2);
20192 assertEquals(b1, b2);
20193 final Matrix ma1 = new Matrix(3, 3);
20194 ma1.setSubmatrix(0, 0,
20195 2, 2,
20196 new double[]{sx, myx, mzx,
20197 mxy, sy, mzy,
20198 mxz, myz, sz});
20199 assertEquals(calibrator.getInitialMa(), ma1);
20200 final Matrix ma2 = new Matrix(3, 3);
20201 calibrator.getInitialMa(ma2);
20202 assertEquals(ma1, ma2);
20203 assertSame(calibrator.getMeasurements(), measurements);
20204 assertTrue(calibrator.isCommonAxisUsed());
20205 assertSame(calibrator.getListener(), this);
20206 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20207 assertFalse(calibrator.isReady());
20208 assertFalse(calibrator.isRunning());
20209 assertNull(calibrator.getEstimatedBiases());
20210 assertFalse(calibrator.getEstimatedBiases(null));
20211 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20212 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20213 assertNull(calibrator.getEstimatedBiasFx());
20214 assertNull(calibrator.getEstimatedBiasFy());
20215 assertNull(calibrator.getEstimatedBiasFz());
20216 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20217 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20218 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20219 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20220 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20221 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20222 assertNull(calibrator.getEstimatedMa());
20223 assertNull(calibrator.getEstimatedSx());
20224 assertNull(calibrator.getEstimatedSy());
20225 assertNull(calibrator.getEstimatedSz());
20226 assertNull(calibrator.getEstimatedMxy());
20227 assertNull(calibrator.getEstimatedMxz());
20228 assertNull(calibrator.getEstimatedMyx());
20229 assertNull(calibrator.getEstimatedMyz());
20230 assertNull(calibrator.getEstimatedMzx());
20231 assertNull(calibrator.getEstimatedMzy());
20232 assertNull(calibrator.getEstimatedCovariance());
20233 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20234 assertNotNull(calibrator.getGroundTruthGravityNorm());
20235 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20236 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20237 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20238 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20239 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20240 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20241 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20242
20243
20244 calibrator = null;
20245 try {
20246 calibrator = new KnownGravityNormAccelerometerCalibrator(
20247 -gravityNorm, measurements,
20248 true, ba, ma, this);
20249 fail("IllegalArgumentException expected but not thrown");
20250 } catch (final IllegalArgumentException ignore) {
20251 }
20252 try {
20253 calibrator = new KnownGravityNormAccelerometerCalibrator(
20254 gravityNorm, measurements, true,
20255 new Matrix(1, 1), ma, this);
20256 fail("IllegalArgumentException expected but not thrown");
20257 } catch (final IllegalArgumentException ignore) {
20258 }
20259 try {
20260 calibrator = new KnownGravityNormAccelerometerCalibrator(
20261 gravityNorm, measurements, true,
20262 new Matrix(1, 3), ma, this);
20263 fail("IllegalArgumentException expected but not thrown");
20264 } catch (final IllegalArgumentException ignore) {
20265 }
20266 try {
20267 calibrator = new KnownGravityNormAccelerometerCalibrator(
20268 gravityNorm, measurements, true,
20269 ba, new Matrix(1, 3), this);
20270 fail("IllegalArgumentException expected but not thrown");
20271 } catch (final IllegalArgumentException ignore) {
20272 }
20273 try {
20274 calibrator = new KnownGravityNormAccelerometerCalibrator(
20275 gravityNorm, measurements, true,
20276 ba, new Matrix(3, 1), this);
20277 fail("IllegalArgumentException expected but not thrown");
20278 } catch (final IllegalArgumentException ignore) {
20279 }
20280 assertNull(calibrator);
20281 }
20282
20283 @Test
20284 public void testConstructor157() throws WrongSizeException {
20285 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20286 final double latitude = Math.toRadians(
20287 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20288 final double longitude = Math.toRadians(
20289 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20290 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20291 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20292 final NEDVelocity nedVelocity = new NEDVelocity();
20293 final ECEFPosition ecefPosition = new ECEFPosition();
20294 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20295 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20296 ecefPosition, ecefVelocity);
20297 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20298 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20299 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20300
20301 KnownGravityNormAccelerometerCalibrator calibrator =
20302 new KnownGravityNormAccelerometerCalibrator(gravityNorm);
20303
20304
20305 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20306 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20307 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20308 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20309 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20310 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20311 final Acceleration bx2 = new Acceleration(0.0,
20312 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20313 calibrator.getInitialBiasXAsAcceleration(bx2);
20314 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20315 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20316 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20317 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20318 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20319 final Acceleration by2 = new Acceleration(0.0,
20320 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20321 calibrator.getInitialBiasYAsAcceleration(by2);
20322 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20323 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20324 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20325 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20326 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20327 final Acceleration bz2 = new Acceleration(0.0,
20328 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20329 calibrator.getInitialBiasZAsAcceleration(bz2);
20330 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20331 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20332 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20333 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20334 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20335 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20336 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20337 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20338 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20339 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20340 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20341 final double[] bias1 = calibrator.getInitialBias();
20342 assertArrayEquals(bias1, new double[3], 0.0);
20343 final double[] bias2 = new double[3];
20344 calibrator.getInitialBias(bias2);
20345 assertArrayEquals(bias1, bias2, 0.0);
20346 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20347 assertEquals(b1, new Matrix(3, 1));
20348 final Matrix b2 = new Matrix(3, 1);
20349 calibrator.getInitialBiasAsMatrix(b2);
20350 assertEquals(b1, b2);
20351 final Matrix ma1 = calibrator.getInitialMa();
20352 assertEquals(ma1, new Matrix(3, 3));
20353 final Matrix ma2 = new Matrix(3, 3);
20354 calibrator.getInitialMa(ma2);
20355 assertEquals(ma1, ma2);
20356 assertNull(calibrator.getMeasurements());
20357 assertFalse(calibrator.isCommonAxisUsed());
20358 assertNull(calibrator.getListener());
20359 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20360 assertFalse(calibrator.isReady());
20361 assertFalse(calibrator.isRunning());
20362 assertNull(calibrator.getEstimatedBiases());
20363 assertFalse(calibrator.getEstimatedBiases(null));
20364 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20365 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20366 assertNull(calibrator.getEstimatedBiasFx());
20367 assertNull(calibrator.getEstimatedBiasFy());
20368 assertNull(calibrator.getEstimatedBiasFz());
20369 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20370 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20371 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20372 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20373 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20374 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20375 assertNull(calibrator.getEstimatedMa());
20376 assertNull(calibrator.getEstimatedSx());
20377 assertNull(calibrator.getEstimatedSy());
20378 assertNull(calibrator.getEstimatedSz());
20379 assertNull(calibrator.getEstimatedMxy());
20380 assertNull(calibrator.getEstimatedMxz());
20381 assertNull(calibrator.getEstimatedMyx());
20382 assertNull(calibrator.getEstimatedMyz());
20383 assertNull(calibrator.getEstimatedMzx());
20384 assertNull(calibrator.getEstimatedMzy());
20385 assertNull(calibrator.getEstimatedCovariance());
20386 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20387 assertNotNull(calibrator.getGroundTruthGravityNorm());
20388 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20389 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20390 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20391 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20392 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20393 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20394 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20395
20396
20397 final Acceleration invalidGravityNorm = new Acceleration(
20398 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20399
20400 calibrator = null;
20401 try {
20402 calibrator = new KnownGravityNormAccelerometerCalibrator(
20403 invalidGravityNorm);
20404 fail("IllegalArgumentException expected but not thrown");
20405 } catch (final IllegalArgumentException ignore) {
20406 }
20407 assertNull(calibrator);
20408 }
20409
20410 @Test
20411 public void testConstructor158() throws WrongSizeException {
20412 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20413 final double latitude = Math.toRadians(
20414 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20415 final double longitude = Math.toRadians(
20416 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20417 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20418 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20419 final NEDVelocity nedVelocity = new NEDVelocity();
20420 final ECEFPosition ecefPosition = new ECEFPosition();
20421 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20422 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20423 ecefPosition, ecefVelocity);
20424 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20425 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20426 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20427
20428 KnownGravityNormAccelerometerCalibrator calibrator =
20429 new KnownGravityNormAccelerometerCalibrator(
20430 gravityNorm, this);
20431
20432
20433 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20434 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20435 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20436 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20437 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20438 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20439 final Acceleration bx2 = new Acceleration(0.0,
20440 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20441 calibrator.getInitialBiasXAsAcceleration(bx2);
20442 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20443 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20444 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20445 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20446 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20447 final Acceleration by2 = new Acceleration(0.0,
20448 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20449 calibrator.getInitialBiasYAsAcceleration(by2);
20450 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20451 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20452 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20453 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20454 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20455 final Acceleration bz2 = new Acceleration(0.0,
20456 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20457 calibrator.getInitialBiasZAsAcceleration(bz2);
20458 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20459 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20460 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20461 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20462 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20463 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20464 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20465 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20466 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20467 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20468 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20469 final double[] bias1 = calibrator.getInitialBias();
20470 assertArrayEquals(bias1, new double[3], 0.0);
20471 final double[] bias2 = new double[3];
20472 calibrator.getInitialBias(bias2);
20473 assertArrayEquals(bias1, bias2, 0.0);
20474 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20475 assertEquals(b1, new Matrix(3, 1));
20476 final Matrix b2 = new Matrix(3, 1);
20477 calibrator.getInitialBiasAsMatrix(b2);
20478 assertEquals(b1, b2);
20479 final Matrix ma1 = calibrator.getInitialMa();
20480 assertEquals(ma1, new Matrix(3, 3));
20481 final Matrix ma2 = new Matrix(3, 3);
20482 calibrator.getInitialMa(ma2);
20483 assertEquals(ma1, ma2);
20484 assertNull(calibrator.getMeasurements());
20485 assertFalse(calibrator.isCommonAxisUsed());
20486 assertSame(calibrator.getListener(), this);
20487 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20488 assertFalse(calibrator.isReady());
20489 assertFalse(calibrator.isRunning());
20490 assertNull(calibrator.getEstimatedBiases());
20491 assertFalse(calibrator.getEstimatedBiases(null));
20492 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20493 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20494 assertNull(calibrator.getEstimatedBiasFx());
20495 assertNull(calibrator.getEstimatedBiasFy());
20496 assertNull(calibrator.getEstimatedBiasFz());
20497 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20498 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20499 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20500 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20501 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20502 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20503 assertNull(calibrator.getEstimatedMa());
20504 assertNull(calibrator.getEstimatedSx());
20505 assertNull(calibrator.getEstimatedSy());
20506 assertNull(calibrator.getEstimatedSz());
20507 assertNull(calibrator.getEstimatedMxy());
20508 assertNull(calibrator.getEstimatedMxz());
20509 assertNull(calibrator.getEstimatedMyx());
20510 assertNull(calibrator.getEstimatedMyz());
20511 assertNull(calibrator.getEstimatedMzx());
20512 assertNull(calibrator.getEstimatedMzy());
20513 assertNull(calibrator.getEstimatedCovariance());
20514 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20515 assertNotNull(calibrator.getGroundTruthGravityNorm());
20516 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20517 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20518 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20519 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20520 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20521 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20522 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20523
20524
20525 final Acceleration invalidGravityNorm = new Acceleration(
20526 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20527
20528 calibrator = null;
20529 try {
20530 calibrator = new KnownGravityNormAccelerometerCalibrator(
20531 invalidGravityNorm, this);
20532 fail("IllegalArgumentException expected but not thrown");
20533 } catch (final IllegalArgumentException ignore) {
20534 }
20535 assertNull(calibrator);
20536 }
20537
20538 @Test
20539 public void testConstructor159() throws WrongSizeException {
20540 final Collection<StandardDeviationBodyKinematics> measurements =
20541 Collections.emptyList();
20542
20543 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20544 final double latitude = Math.toRadians(
20545 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20546 final double longitude = Math.toRadians(
20547 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20548 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20549 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20550 final NEDVelocity nedVelocity = new NEDVelocity();
20551 final ECEFPosition ecefPosition = new ECEFPosition();
20552 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20553 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20554 ecefPosition, ecefVelocity);
20555 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20556 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20557 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20558
20559 KnownGravityNormAccelerometerCalibrator calibrator =
20560 new KnownGravityNormAccelerometerCalibrator(
20561 gravityNorm, measurements);
20562
20563
20564 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20565 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20566 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20567 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20568 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20569 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20570 final Acceleration bx2 = new Acceleration(0.0,
20571 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20572 calibrator.getInitialBiasXAsAcceleration(bx2);
20573 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20574 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20575 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20576 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20577 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20578 final Acceleration by2 = new Acceleration(0.0,
20579 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20580 calibrator.getInitialBiasYAsAcceleration(by2);
20581 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20582 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20583 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20584 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20585 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20586 final Acceleration bz2 = new Acceleration(0.0,
20587 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20588 calibrator.getInitialBiasZAsAcceleration(bz2);
20589 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20590 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20591 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20592 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20593 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20594 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20595 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20596 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20597 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20598 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20599 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20600 final double[] bias1 = calibrator.getInitialBias();
20601 assertArrayEquals(bias1, new double[3], 0.0);
20602 final double[] bias2 = new double[3];
20603 calibrator.getInitialBias(bias2);
20604 assertArrayEquals(bias1, bias2, 0.0);
20605 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20606 assertEquals(b1, new Matrix(3, 1));
20607 final Matrix b2 = new Matrix(3, 1);
20608 calibrator.getInitialBiasAsMatrix(b2);
20609 assertEquals(b1, b2);
20610 final Matrix ma1 = calibrator.getInitialMa();
20611 assertEquals(ma1, new Matrix(3, 3));
20612 final Matrix ma2 = new Matrix(3, 3);
20613 calibrator.getInitialMa(ma2);
20614 assertEquals(ma1, ma2);
20615 assertSame(calibrator.getMeasurements(), measurements);
20616 assertFalse(calibrator.isCommonAxisUsed());
20617 assertNull(calibrator.getListener());
20618 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20619 assertFalse(calibrator.isReady());
20620 assertFalse(calibrator.isRunning());
20621 assertNull(calibrator.getEstimatedBiases());
20622 assertFalse(calibrator.getEstimatedBiases(null));
20623 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20624 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20625 assertNull(calibrator.getEstimatedBiasFx());
20626 assertNull(calibrator.getEstimatedBiasFy());
20627 assertNull(calibrator.getEstimatedBiasFz());
20628 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20629 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20630 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20631 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20632 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20633 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20634 assertNull(calibrator.getEstimatedMa());
20635 assertNull(calibrator.getEstimatedSx());
20636 assertNull(calibrator.getEstimatedSy());
20637 assertNull(calibrator.getEstimatedSz());
20638 assertNull(calibrator.getEstimatedMxy());
20639 assertNull(calibrator.getEstimatedMxz());
20640 assertNull(calibrator.getEstimatedMyx());
20641 assertNull(calibrator.getEstimatedMyz());
20642 assertNull(calibrator.getEstimatedMzx());
20643 assertNull(calibrator.getEstimatedMzy());
20644 assertNull(calibrator.getEstimatedCovariance());
20645 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20646 assertNotNull(calibrator.getGroundTruthGravityNorm());
20647 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20648 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20649 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20650 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20651 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20652 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20653 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20654
20655
20656 final Acceleration invalidGravityNorm = new Acceleration(
20657 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20658
20659 calibrator = null;
20660 try {
20661 calibrator = new KnownGravityNormAccelerometerCalibrator(
20662 invalidGravityNorm, measurements);
20663 fail("IllegalArgumentException expected but not thrown");
20664 } catch (final IllegalArgumentException ignore) {
20665 }
20666 assertNull(calibrator);
20667 }
20668
20669 @Test
20670 public void testConstructor160() throws WrongSizeException {
20671 final Collection<StandardDeviationBodyKinematics> measurements =
20672 Collections.emptyList();
20673
20674 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20675 final double latitude = Math.toRadians(
20676 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20677 final double longitude = Math.toRadians(
20678 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20679 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20680 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20681 final NEDVelocity nedVelocity = new NEDVelocity();
20682 final ECEFPosition ecefPosition = new ECEFPosition();
20683 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20684 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20685 ecefPosition, ecefVelocity);
20686 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20687 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20688 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20689
20690 KnownGravityNormAccelerometerCalibrator calibrator =
20691 new KnownGravityNormAccelerometerCalibrator(
20692 gravityNorm, measurements, this);
20693
20694
20695 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20696 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20697 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20698 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20699 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20700 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20701 final Acceleration bx2 = new Acceleration(0.0,
20702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20703 calibrator.getInitialBiasXAsAcceleration(bx2);
20704 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20705 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20707 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20708 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20709 final Acceleration by2 = new Acceleration(0.0,
20710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20711 calibrator.getInitialBiasYAsAcceleration(by2);
20712 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20713 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20714 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20715 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20716 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20717 final Acceleration bz2 = new Acceleration(0.0,
20718 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20719 calibrator.getInitialBiasZAsAcceleration(bz2);
20720 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20721 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20722 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20723 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20724 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20725 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20726 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20727 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20728 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20729 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20730 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20731 final double[] bias1 = calibrator.getInitialBias();
20732 assertArrayEquals(bias1, new double[3], 0.0);
20733 final double[] bias2 = new double[3];
20734 calibrator.getInitialBias(bias2);
20735 assertArrayEquals(bias1, bias2, 0.0);
20736 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20737 assertEquals(b1, new Matrix(3, 1));
20738 final Matrix b2 = new Matrix(3, 1);
20739 calibrator.getInitialBiasAsMatrix(b2);
20740 assertEquals(b1, b2);
20741 final Matrix ma1 = calibrator.getInitialMa();
20742 assertEquals(ma1, new Matrix(3, 3));
20743 final Matrix ma2 = new Matrix(3, 3);
20744 calibrator.getInitialMa(ma2);
20745 assertEquals(ma1, ma2);
20746 assertSame(calibrator.getMeasurements(), measurements);
20747 assertFalse(calibrator.isCommonAxisUsed());
20748 assertSame(calibrator.getListener(), this);
20749 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20750 assertFalse(calibrator.isReady());
20751 assertFalse(calibrator.isRunning());
20752 assertNull(calibrator.getEstimatedBiases());
20753 assertFalse(calibrator.getEstimatedBiases(null));
20754 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20755 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20756 assertNull(calibrator.getEstimatedBiasFx());
20757 assertNull(calibrator.getEstimatedBiasFy());
20758 assertNull(calibrator.getEstimatedBiasFz());
20759 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20760 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20761 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20762 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20763 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20764 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20765 assertNull(calibrator.getEstimatedMa());
20766 assertNull(calibrator.getEstimatedSx());
20767 assertNull(calibrator.getEstimatedSy());
20768 assertNull(calibrator.getEstimatedSz());
20769 assertNull(calibrator.getEstimatedMxy());
20770 assertNull(calibrator.getEstimatedMxz());
20771 assertNull(calibrator.getEstimatedMyx());
20772 assertNull(calibrator.getEstimatedMyz());
20773 assertNull(calibrator.getEstimatedMzx());
20774 assertNull(calibrator.getEstimatedMzy());
20775 assertNull(calibrator.getEstimatedCovariance());
20776 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20777 assertNotNull(calibrator.getGroundTruthGravityNorm());
20778 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20779 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20780 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20781 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20782 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20783 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20784 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20785
20786
20787 final Acceleration invalidGravityNorm = new Acceleration(
20788 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20789
20790 calibrator = null;
20791 try {
20792 calibrator = new KnownGravityNormAccelerometerCalibrator(
20793 invalidGravityNorm, measurements, this);
20794 fail("IllegalArgumentException expected but not thrown");
20795 } catch (final IllegalArgumentException ignore) {
20796 }
20797 assertNull(calibrator);
20798 }
20799
20800 @Test
20801 public void testConstructor161() throws WrongSizeException {
20802 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20803 final double latitude = Math.toRadians(
20804 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20805 final double longitude = Math.toRadians(
20806 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20807 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20808 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20809 final NEDVelocity nedVelocity = new NEDVelocity();
20810 final ECEFPosition ecefPosition = new ECEFPosition();
20811 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20812 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20813 ecefPosition, ecefVelocity);
20814 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20815 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20816 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20817
20818 KnownGravityNormAccelerometerCalibrator calibrator =
20819 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
20820 true);
20821
20822
20823 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20824 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20825 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20826 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20827 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20828 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20829 final Acceleration bx2 = new Acceleration(0.0,
20830 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20831 calibrator.getInitialBiasXAsAcceleration(bx2);
20832 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20833 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20834 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20835 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20836 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20837 final Acceleration by2 = new Acceleration(0.0,
20838 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20839 calibrator.getInitialBiasYAsAcceleration(by2);
20840 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20841 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20842 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20843 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20844 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20845 final Acceleration bz2 = new Acceleration(0.0,
20846 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20847 calibrator.getInitialBiasZAsAcceleration(bz2);
20848 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20849 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20850 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20851 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20852 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20853 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20854 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20855 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20856 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20857 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20858 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20859 final double[] bias1 = calibrator.getInitialBias();
20860 assertArrayEquals(bias1, new double[3], 0.0);
20861 final double[] bias2 = new double[3];
20862 calibrator.getInitialBias(bias2);
20863 assertArrayEquals(bias1, bias2, 0.0);
20864 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20865 assertEquals(b1, new Matrix(3, 1));
20866 final Matrix b2 = new Matrix(3, 1);
20867 calibrator.getInitialBiasAsMatrix(b2);
20868 assertEquals(b1, b2);
20869 final Matrix ma1 = calibrator.getInitialMa();
20870 assertEquals(ma1, new Matrix(3, 3));
20871 final Matrix ma2 = new Matrix(3, 3);
20872 calibrator.getInitialMa(ma2);
20873 assertEquals(ma1, ma2);
20874 assertNull(calibrator.getMeasurements());
20875 assertTrue(calibrator.isCommonAxisUsed());
20876 assertNull(calibrator.getListener());
20877 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20878 assertFalse(calibrator.isReady());
20879 assertFalse(calibrator.isRunning());
20880 assertNull(calibrator.getEstimatedBiases());
20881 assertFalse(calibrator.getEstimatedBiases(null));
20882 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20883 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20884 assertNull(calibrator.getEstimatedBiasFx());
20885 assertNull(calibrator.getEstimatedBiasFy());
20886 assertNull(calibrator.getEstimatedBiasFz());
20887 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20888 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20889 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20890 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20891 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20892 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20893 assertNull(calibrator.getEstimatedMa());
20894 assertNull(calibrator.getEstimatedSx());
20895 assertNull(calibrator.getEstimatedSy());
20896 assertNull(calibrator.getEstimatedSz());
20897 assertNull(calibrator.getEstimatedMxy());
20898 assertNull(calibrator.getEstimatedMxz());
20899 assertNull(calibrator.getEstimatedMyx());
20900 assertNull(calibrator.getEstimatedMyz());
20901 assertNull(calibrator.getEstimatedMzx());
20902 assertNull(calibrator.getEstimatedMzy());
20903 assertNull(calibrator.getEstimatedCovariance());
20904 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20905 assertNotNull(calibrator.getGroundTruthGravityNorm());
20906 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20907 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20908 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20909 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20910 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20911 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20912 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20913
20914
20915 final Acceleration invalidGravityNorm = new Acceleration(
20916 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20917
20918 calibrator = null;
20919 try {
20920 calibrator = new KnownGravityNormAccelerometerCalibrator(
20921 invalidGravityNorm, true);
20922 fail("IllegalArgumentException expected but not thrown");
20923 } catch (final IllegalArgumentException ignore) {
20924 }
20925 assertNull(calibrator);
20926 }
20927
20928 @Test
20929 public void testConstructor162() throws WrongSizeException {
20930 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20931 final double latitude = Math.toRadians(
20932 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20933 final double longitude = Math.toRadians(
20934 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20935 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20936 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20937 final NEDVelocity nedVelocity = new NEDVelocity();
20938 final ECEFPosition ecefPosition = new ECEFPosition();
20939 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20940 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20941 ecefPosition, ecefVelocity);
20942 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20943 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20944 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20945
20946 KnownGravityNormAccelerometerCalibrator calibrator =
20947 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
20948 true, this);
20949
20950
20951 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20952 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20953 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20954 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20955 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20956 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20957 final Acceleration bx2 = new Acceleration(0.0,
20958 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20959 calibrator.getInitialBiasXAsAcceleration(bx2);
20960 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20961 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20962 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20963 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20964 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20965 final Acceleration by2 = new Acceleration(0.0,
20966 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20967 calibrator.getInitialBiasYAsAcceleration(by2);
20968 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20969 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20970 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20971 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20972 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20973 final Acceleration bz2 = new Acceleration(0.0,
20974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20975 calibrator.getInitialBiasZAsAcceleration(bz2);
20976 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20977 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20978 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20979 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20980 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20981 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20982 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20983 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20984 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20985 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20986 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20987 final double[] bias1 = calibrator.getInitialBias();
20988 assertArrayEquals(bias1, new double[3], 0.0);
20989 final double[] bias2 = new double[3];
20990 calibrator.getInitialBias(bias2);
20991 assertArrayEquals(bias1, bias2, 0.0);
20992 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20993 assertEquals(b1, new Matrix(3, 1));
20994 final Matrix b2 = new Matrix(3, 1);
20995 calibrator.getInitialBiasAsMatrix(b2);
20996 assertEquals(b1, b2);
20997 final Matrix ma1 = calibrator.getInitialMa();
20998 assertEquals(ma1, new Matrix(3, 3));
20999 final Matrix ma2 = new Matrix(3, 3);
21000 calibrator.getInitialMa(ma2);
21001 assertEquals(ma1, ma2);
21002 assertNull(calibrator.getMeasurements());
21003 assertTrue(calibrator.isCommonAxisUsed());
21004 assertSame(calibrator.getListener(), this);
21005 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21006 assertFalse(calibrator.isReady());
21007 assertFalse(calibrator.isRunning());
21008 assertNull(calibrator.getEstimatedBiases());
21009 assertFalse(calibrator.getEstimatedBiases(null));
21010 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21011 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21012 assertNull(calibrator.getEstimatedBiasFx());
21013 assertNull(calibrator.getEstimatedBiasFy());
21014 assertNull(calibrator.getEstimatedBiasFz());
21015 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21016 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21017 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21018 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21019 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21020 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21021 assertNull(calibrator.getEstimatedMa());
21022 assertNull(calibrator.getEstimatedSx());
21023 assertNull(calibrator.getEstimatedSy());
21024 assertNull(calibrator.getEstimatedSz());
21025 assertNull(calibrator.getEstimatedMxy());
21026 assertNull(calibrator.getEstimatedMxz());
21027 assertNull(calibrator.getEstimatedMyx());
21028 assertNull(calibrator.getEstimatedMyz());
21029 assertNull(calibrator.getEstimatedMzx());
21030 assertNull(calibrator.getEstimatedMzy());
21031 assertNull(calibrator.getEstimatedCovariance());
21032 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21033 assertNotNull(calibrator.getGroundTruthGravityNorm());
21034 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21035 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21036 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21037 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21038 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21039 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21040 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21041
21042
21043 final Acceleration invalidGravityNorm = new Acceleration(
21044 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21045
21046 calibrator = null;
21047 try {
21048 calibrator = new KnownGravityNormAccelerometerCalibrator(
21049 invalidGravityNorm, true, this);
21050 fail("IllegalArgumentException expected but not thrown");
21051 } catch (final IllegalArgumentException ignore) {
21052 }
21053 assertNull(calibrator);
21054 }
21055
21056 @Test
21057 public void testConstructor163() throws WrongSizeException {
21058 final Collection<StandardDeviationBodyKinematics> measurements =
21059 Collections.emptyList();
21060 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21061 final double latitude = Math.toRadians(
21062 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21063 final double longitude = Math.toRadians(
21064 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21065 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21066 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21067 final NEDVelocity nedVelocity = new NEDVelocity();
21068 final ECEFPosition ecefPosition = new ECEFPosition();
21069 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21070 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21071 ecefPosition, ecefVelocity);
21072 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21073 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21074 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21075
21076 KnownGravityNormAccelerometerCalibrator calibrator =
21077 new KnownGravityNormAccelerometerCalibrator(
21078 gravityNorm, measurements,
21079 true);
21080
21081
21082 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
21083 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
21084 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
21085 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21086 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
21087 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21088 final Acceleration bx2 = new Acceleration(0.0,
21089 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21090 calibrator.getInitialBiasXAsAcceleration(bx2);
21091 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
21092 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21093 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21094 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
21095 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21096 final Acceleration by2 = new Acceleration(0.0,
21097 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21098 calibrator.getInitialBiasYAsAcceleration(by2);
21099 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
21100 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21101 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21102 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
21103 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21104 final Acceleration bz2 = new Acceleration(0.0,
21105 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21106 calibrator.getInitialBiasZAsAcceleration(bz2);
21107 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
21108 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21109 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21110 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21111 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21112 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21113 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21114 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21115 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21116 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21117 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21118 final double[] bias1 = calibrator.getInitialBias();
21119 assertArrayEquals(bias1, new double[3], 0.0);
21120 final double[] bias2 = new double[3];
21121 calibrator.getInitialBias(bias2);
21122 assertArrayEquals(bias1, bias2, 0.0);
21123 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21124 assertEquals(b1, new Matrix(3, 1));
21125 final Matrix b2 = new Matrix(3, 1);
21126 calibrator.getInitialBiasAsMatrix(b2);
21127 assertEquals(b1, b2);
21128 final Matrix ma1 = calibrator.getInitialMa();
21129 assertEquals(ma1, new Matrix(3, 3));
21130 final Matrix ma2 = new Matrix(3, 3);
21131 calibrator.getInitialMa(ma2);
21132 assertEquals(ma1, ma2);
21133 assertSame(calibrator.getMeasurements(), measurements);
21134 assertTrue(calibrator.isCommonAxisUsed());
21135 assertNull(calibrator.getListener());
21136 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21137 assertFalse(calibrator.isReady());
21138 assertFalse(calibrator.isRunning());
21139 assertNull(calibrator.getEstimatedBiases());
21140 assertFalse(calibrator.getEstimatedBiases(null));
21141 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21142 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21143 assertNull(calibrator.getEstimatedBiasFx());
21144 assertNull(calibrator.getEstimatedBiasFy());
21145 assertNull(calibrator.getEstimatedBiasFz());
21146 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21147 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21148 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21149 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21150 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21151 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21152 assertNull(calibrator.getEstimatedMa());
21153 assertNull(calibrator.getEstimatedSx());
21154 assertNull(calibrator.getEstimatedSy());
21155 assertNull(calibrator.getEstimatedSz());
21156 assertNull(calibrator.getEstimatedMxy());
21157 assertNull(calibrator.getEstimatedMxz());
21158 assertNull(calibrator.getEstimatedMyx());
21159 assertNull(calibrator.getEstimatedMyz());
21160 assertNull(calibrator.getEstimatedMzx());
21161 assertNull(calibrator.getEstimatedMzy());
21162 assertNull(calibrator.getEstimatedCovariance());
21163 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21164 assertNotNull(calibrator.getGroundTruthGravityNorm());
21165 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21166 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21167 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21168 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21169 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21170 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21171 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21172
21173
21174 final Acceleration invalidGravityNorm = new Acceleration(
21175 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21176
21177 calibrator = null;
21178 try {
21179 calibrator = new KnownGravityNormAccelerometerCalibrator(
21180 invalidGravityNorm, measurements,
21181 true);
21182 fail("IllegalArgumentException expected but not thrown");
21183 } catch (final IllegalArgumentException ignore) {
21184 }
21185 assertNull(calibrator);
21186 }
21187
21188 @Test
21189 public void testConstructor164() throws WrongSizeException {
21190 final Collection<StandardDeviationBodyKinematics> measurements =
21191 Collections.emptyList();
21192 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21193 final double latitude = Math.toRadians(
21194 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21195 final double longitude = Math.toRadians(
21196 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21197 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21198 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21199 final NEDVelocity nedVelocity = new NEDVelocity();
21200 final ECEFPosition ecefPosition = new ECEFPosition();
21201 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21202 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21203 ecefPosition, ecefVelocity);
21204 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21205 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21206 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21207
21208 KnownGravityNormAccelerometerCalibrator calibrator =
21209 new KnownGravityNormAccelerometerCalibrator(
21210 gravityNorm, measurements,
21211 true, this);
21212
21213
21214 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
21215 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
21216 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
21217 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21218 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
21219 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21220 final Acceleration bx2 = new Acceleration(0.0,
21221 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21222 calibrator.getInitialBiasXAsAcceleration(bx2);
21223 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
21224 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21225 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21226 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
21227 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21228 final Acceleration by2 = new Acceleration(0.0,
21229 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21230 calibrator.getInitialBiasYAsAcceleration(by2);
21231 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
21232 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21233 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21234 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
21235 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21236 final Acceleration bz2 = new Acceleration(0.0,
21237 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21238 calibrator.getInitialBiasZAsAcceleration(bz2);
21239 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
21240 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21241 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21242 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21243 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21244 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21245 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21246 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21247 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21248 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21249 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21250 final double[] bias1 = calibrator.getInitialBias();
21251 assertArrayEquals(bias1, new double[3], 0.0);
21252 final double[] bias2 = new double[3];
21253 calibrator.getInitialBias(bias2);
21254 assertArrayEquals(bias1, bias2, 0.0);
21255 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21256 assertEquals(b1, new Matrix(3, 1));
21257 final Matrix b2 = new Matrix(3, 1);
21258 calibrator.getInitialBiasAsMatrix(b2);
21259 assertEquals(b1, b2);
21260 final Matrix ma1 = calibrator.getInitialMa();
21261 assertEquals(ma1, new Matrix(3, 3));
21262 final Matrix ma2 = new Matrix(3, 3);
21263 calibrator.getInitialMa(ma2);
21264 assertEquals(ma1, ma2);
21265 assertSame(calibrator.getMeasurements(), measurements);
21266 assertTrue(calibrator.isCommonAxisUsed());
21267 assertSame(calibrator.getListener(), this);
21268 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21269 assertFalse(calibrator.isReady());
21270 assertFalse(calibrator.isRunning());
21271 assertNull(calibrator.getEstimatedBiases());
21272 assertFalse(calibrator.getEstimatedBiases(null));
21273 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21274 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21275 assertNull(calibrator.getEstimatedBiasFx());
21276 assertNull(calibrator.getEstimatedBiasFy());
21277 assertNull(calibrator.getEstimatedBiasFz());
21278 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21279 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21280 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21281 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21282 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21283 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21284 assertNull(calibrator.getEstimatedMa());
21285 assertNull(calibrator.getEstimatedSx());
21286 assertNull(calibrator.getEstimatedSy());
21287 assertNull(calibrator.getEstimatedSz());
21288 assertNull(calibrator.getEstimatedMxy());
21289 assertNull(calibrator.getEstimatedMxz());
21290 assertNull(calibrator.getEstimatedMyx());
21291 assertNull(calibrator.getEstimatedMyz());
21292 assertNull(calibrator.getEstimatedMzx());
21293 assertNull(calibrator.getEstimatedMzy());
21294 assertNull(calibrator.getEstimatedCovariance());
21295 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21296 assertNotNull(calibrator.getGroundTruthGravityNorm());
21297 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21298 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21299 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21300 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21301 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21302 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21303 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21304
21305
21306 final Acceleration invalidGravityNorm = new Acceleration(
21307 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21308
21309 calibrator = null;
21310 try {
21311 calibrator = new KnownGravityNormAccelerometerCalibrator(
21312 invalidGravityNorm, measurements,
21313 true, this);
21314 fail("IllegalArgumentException expected but not thrown");
21315 } catch (final IllegalArgumentException ignore) {
21316 }
21317 assertNull(calibrator);
21318 }
21319
21320 @Test
21321 public void testConstructor165() throws WrongSizeException {
21322 final Matrix ba = generateBa();
21323 final double biasX = ba.getElementAtIndex(0);
21324 final double biasY = ba.getElementAtIndex(1);
21325 final double biasZ = ba.getElementAtIndex(2);
21326
21327 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21328 final double latitude = Math.toRadians(
21329 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21330 final double longitude = Math.toRadians(
21331 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21332 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21333 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21334 final NEDVelocity nedVelocity = new NEDVelocity();
21335 final ECEFPosition ecefPosition = new ECEFPosition();
21336 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21337 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21338 ecefPosition, ecefVelocity);
21339 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21340 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21341 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21342
21343 KnownGravityNormAccelerometerCalibrator calibrator =
21344 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
21345 biasX, biasY, biasZ);
21346
21347
21348 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21349 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21350 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21351 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21352 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21353 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21354 final Acceleration bx2 = new Acceleration(0.0,
21355 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21356 calibrator.getInitialBiasXAsAcceleration(bx2);
21357 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21358 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21359 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21360 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21361 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21362 final Acceleration by2 = new Acceleration(0.0,
21363 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21364 calibrator.getInitialBiasYAsAcceleration(by2);
21365 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21366 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21367 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21368 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21369 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21370 final Acceleration bz2 = new Acceleration(0.0,
21371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21372 calibrator.getInitialBiasZAsAcceleration(bz2);
21373 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21374 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21375 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21376 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21377 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21378 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21379 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21380 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21381 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21382 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21383 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21384 final double[] bias1 = calibrator.getInitialBias();
21385 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21386 final double[] bias2 = new double[3];
21387 calibrator.getInitialBias(bias2);
21388 assertArrayEquals(bias1, bias2, 0.0);
21389 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21390 assertEquals(b1, ba);
21391 final Matrix b2 = new Matrix(3, 1);
21392 calibrator.getInitialBiasAsMatrix(b2);
21393 assertEquals(b1, b2);
21394 final Matrix ma1 = calibrator.getInitialMa();
21395 assertEquals(ma1, new Matrix(3, 3));
21396 final Matrix ma2 = new Matrix(3, 3);
21397 calibrator.getInitialMa(ma2);
21398 assertEquals(ma1, ma2);
21399 assertNull(calibrator.getMeasurements());
21400 assertFalse(calibrator.isCommonAxisUsed());
21401 assertNull(calibrator.getListener());
21402 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21403 assertFalse(calibrator.isReady());
21404 assertFalse(calibrator.isRunning());
21405 assertNull(calibrator.getEstimatedBiases());
21406 assertFalse(calibrator.getEstimatedBiases(null));
21407 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21408 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21409 assertNull(calibrator.getEstimatedBiasFx());
21410 assertNull(calibrator.getEstimatedBiasFy());
21411 assertNull(calibrator.getEstimatedBiasFz());
21412 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21413 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21414 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21415 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21416 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21417 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21418 assertNull(calibrator.getEstimatedMa());
21419 assertNull(calibrator.getEstimatedSx());
21420 assertNull(calibrator.getEstimatedSy());
21421 assertNull(calibrator.getEstimatedSz());
21422 assertNull(calibrator.getEstimatedMxy());
21423 assertNull(calibrator.getEstimatedMxz());
21424 assertNull(calibrator.getEstimatedMyx());
21425 assertNull(calibrator.getEstimatedMyz());
21426 assertNull(calibrator.getEstimatedMzx());
21427 assertNull(calibrator.getEstimatedMzy());
21428 assertNull(calibrator.getEstimatedCovariance());
21429 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21430 assertNotNull(calibrator.getGroundTruthGravityNorm());
21431 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21432 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21433 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21434 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21435 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21436 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21437 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21438
21439
21440 final Acceleration invalidGravityNorm = new Acceleration(
21441 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21442
21443 calibrator = null;
21444 try {
21445 calibrator = new KnownGravityNormAccelerometerCalibrator(
21446 invalidGravityNorm, biasX, biasY, biasZ);
21447 fail("IllegalArgumentException expected but not thrown");
21448 } catch (final IllegalArgumentException ignore) {
21449 }
21450 assertNull(calibrator);
21451 }
21452
21453 @Test
21454 public void testConstructor166() throws WrongSizeException {
21455 final Matrix ba = generateBa();
21456 final double biasX = ba.getElementAtIndex(0);
21457 final double biasY = ba.getElementAtIndex(1);
21458 final double biasZ = ba.getElementAtIndex(2);
21459
21460 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21461 final double latitude = Math.toRadians(
21462 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21463 final double longitude = Math.toRadians(
21464 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21465 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21466 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21467 final NEDVelocity nedVelocity = new NEDVelocity();
21468 final ECEFPosition ecefPosition = new ECEFPosition();
21469 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21470 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21471 ecefPosition, ecefVelocity);
21472 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21473 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21474 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21475
21476 KnownGravityNormAccelerometerCalibrator calibrator =
21477 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
21478 biasX, biasY, biasZ, this);
21479
21480
21481 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21482 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21483 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21484 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21485 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21486 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21487 final Acceleration bx2 = new Acceleration(0.0,
21488 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21489 calibrator.getInitialBiasXAsAcceleration(bx2);
21490 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21491 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21492 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21493 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21494 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21495 final Acceleration by2 = new Acceleration(0.0,
21496 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21497 calibrator.getInitialBiasYAsAcceleration(by2);
21498 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21499 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21500 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21501 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21502 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21503 final Acceleration bz2 = new Acceleration(0.0,
21504 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21505 calibrator.getInitialBiasZAsAcceleration(bz2);
21506 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21507 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21508 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21509 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21510 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21511 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21512 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21513 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21514 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21515 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21516 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21517 final double[] bias1 = calibrator.getInitialBias();
21518 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21519 final double[] bias2 = new double[3];
21520 calibrator.getInitialBias(bias2);
21521 assertArrayEquals(bias1, bias2, 0.0);
21522 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21523 assertEquals(b1, ba);
21524 final Matrix b2 = new Matrix(3, 1);
21525 calibrator.getInitialBiasAsMatrix(b2);
21526 assertEquals(b1, b2);
21527 final Matrix ma1 = calibrator.getInitialMa();
21528 assertEquals(ma1, new Matrix(3, 3));
21529 final Matrix ma2 = new Matrix(3, 3);
21530 calibrator.getInitialMa(ma2);
21531 assertEquals(ma1, ma2);
21532 assertNull(calibrator.getMeasurements());
21533 assertFalse(calibrator.isCommonAxisUsed());
21534 assertSame(calibrator.getListener(), this);
21535 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21536 assertFalse(calibrator.isReady());
21537 assertFalse(calibrator.isRunning());
21538 assertNull(calibrator.getEstimatedBiases());
21539 assertFalse(calibrator.getEstimatedBiases(null));
21540 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21541 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21542 assertNull(calibrator.getEstimatedBiasFx());
21543 assertNull(calibrator.getEstimatedBiasFy());
21544 assertNull(calibrator.getEstimatedBiasFz());
21545 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21546 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21547 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21548 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21549 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21550 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21551 assertNull(calibrator.getEstimatedMa());
21552 assertNull(calibrator.getEstimatedSx());
21553 assertNull(calibrator.getEstimatedSy());
21554 assertNull(calibrator.getEstimatedSz());
21555 assertNull(calibrator.getEstimatedMxy());
21556 assertNull(calibrator.getEstimatedMxz());
21557 assertNull(calibrator.getEstimatedMyx());
21558 assertNull(calibrator.getEstimatedMyz());
21559 assertNull(calibrator.getEstimatedMzx());
21560 assertNull(calibrator.getEstimatedMzy());
21561 assertNull(calibrator.getEstimatedCovariance());
21562 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21563 assertNotNull(calibrator.getGroundTruthGravityNorm());
21564 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21565 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21566 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21567 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21568 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21569 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21570 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21571
21572
21573 final Acceleration invalidGravityNorm = new Acceleration(
21574 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21575
21576 calibrator = null;
21577 try {
21578 calibrator = new KnownGravityNormAccelerometerCalibrator(
21579 invalidGravityNorm, biasX, biasY, biasZ,
21580 this);
21581 fail("IllegalArgumentException expected but not thrown");
21582 } catch (final IllegalArgumentException ignore) {
21583 }
21584 assertNull(calibrator);
21585 }
21586
21587 @Test
21588 public void testConstructor167() throws WrongSizeException {
21589 final Collection<StandardDeviationBodyKinematics> measurements =
21590 Collections.emptyList();
21591
21592 final Matrix ba = generateBa();
21593 final double biasX = ba.getElementAtIndex(0);
21594 final double biasY = ba.getElementAtIndex(1);
21595 final double biasZ = ba.getElementAtIndex(2);
21596
21597 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21598 final double latitude = Math.toRadians(
21599 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21600 final double longitude = Math.toRadians(
21601 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21602 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21603 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21604 final NEDVelocity nedVelocity = new NEDVelocity();
21605 final ECEFPosition ecefPosition = new ECEFPosition();
21606 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21607 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21608 ecefPosition, ecefVelocity);
21609 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21610 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21611 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21612
21613 KnownGravityNormAccelerometerCalibrator calibrator =
21614 new KnownGravityNormAccelerometerCalibrator(
21615 gravityNorm, measurements,
21616 biasX, biasY, biasZ);
21617
21618
21619 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21620 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21621 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21622 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21623 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21624 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21625 final Acceleration bx2 = new Acceleration(0.0,
21626 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21627 calibrator.getInitialBiasXAsAcceleration(bx2);
21628 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21629 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21630 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21631 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21632 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21633 final Acceleration by2 = new Acceleration(0.0,
21634 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21635 calibrator.getInitialBiasYAsAcceleration(by2);
21636 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21637 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21638 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21639 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21640 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21641 final Acceleration bz2 = new Acceleration(0.0,
21642 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21643 calibrator.getInitialBiasZAsAcceleration(bz2);
21644 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21645 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21646 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21647 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21648 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21649 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21650 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21651 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21652 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21653 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21654 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21655 final double[] bias1 = calibrator.getInitialBias();
21656 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21657 final double[] bias2 = new double[3];
21658 calibrator.getInitialBias(bias2);
21659 assertArrayEquals(bias1, bias2, 0.0);
21660 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21661 assertEquals(b1, ba);
21662 final Matrix b2 = new Matrix(3, 1);
21663 calibrator.getInitialBiasAsMatrix(b2);
21664 assertEquals(b1, b2);
21665 final Matrix ma1 = calibrator.getInitialMa();
21666 assertEquals(ma1, new Matrix(3, 3));
21667 final Matrix ma2 = new Matrix(3, 3);
21668 calibrator.getInitialMa(ma2);
21669 assertEquals(ma1, ma2);
21670 assertSame(calibrator.getMeasurements(), measurements);
21671 assertFalse(calibrator.isCommonAxisUsed());
21672 assertNull(calibrator.getListener());
21673 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21674 assertFalse(calibrator.isReady());
21675 assertFalse(calibrator.isRunning());
21676 assertNull(calibrator.getEstimatedBiases());
21677 assertFalse(calibrator.getEstimatedBiases(null));
21678 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21679 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21680 assertNull(calibrator.getEstimatedBiasFx());
21681 assertNull(calibrator.getEstimatedBiasFy());
21682 assertNull(calibrator.getEstimatedBiasFz());
21683 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21684 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21685 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21686 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21687 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21688 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21689 assertNull(calibrator.getEstimatedMa());
21690 assertNull(calibrator.getEstimatedSx());
21691 assertNull(calibrator.getEstimatedSy());
21692 assertNull(calibrator.getEstimatedSz());
21693 assertNull(calibrator.getEstimatedMxy());
21694 assertNull(calibrator.getEstimatedMxz());
21695 assertNull(calibrator.getEstimatedMyx());
21696 assertNull(calibrator.getEstimatedMyz());
21697 assertNull(calibrator.getEstimatedMzx());
21698 assertNull(calibrator.getEstimatedMzy());
21699 assertNull(calibrator.getEstimatedCovariance());
21700 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21701 assertNotNull(calibrator.getGroundTruthGravityNorm());
21702 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21703 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21704 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21705 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21706 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21707 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21708 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21709
21710
21711 final Acceleration invalidGravityNorm = new Acceleration(
21712 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21713
21714 calibrator = null;
21715 try {
21716 calibrator = new KnownGravityNormAccelerometerCalibrator(
21717 invalidGravityNorm, measurements,
21718 biasX, biasY, biasZ);
21719 fail("IllegalArgumentException expected but not thrown");
21720 } catch (final IllegalArgumentException ignore) {
21721 }
21722 assertNull(calibrator);
21723 }
21724
21725 @Test
21726 public void testConstructor168() throws WrongSizeException {
21727 final Collection<StandardDeviationBodyKinematics> measurements =
21728 Collections.emptyList();
21729
21730 final Matrix ba = generateBa();
21731 final double biasX = ba.getElementAtIndex(0);
21732 final double biasY = ba.getElementAtIndex(1);
21733 final double biasZ = ba.getElementAtIndex(2);
21734
21735 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21736 final double latitude = Math.toRadians(
21737 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21738 final double longitude = Math.toRadians(
21739 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21740 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21741 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21742 final NEDVelocity nedVelocity = new NEDVelocity();
21743 final ECEFPosition ecefPosition = new ECEFPosition();
21744 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21745 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21746 ecefPosition, ecefVelocity);
21747 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21748 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21749 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21750
21751 KnownGravityNormAccelerometerCalibrator calibrator =
21752 new KnownGravityNormAccelerometerCalibrator(
21753 gravityNorm, measurements,
21754 biasX, biasY, biasZ, this);
21755
21756
21757 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21758 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21759 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21760 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21761 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21762 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21763 final Acceleration bx2 = new Acceleration(0.0,
21764 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21765 calibrator.getInitialBiasXAsAcceleration(bx2);
21766 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21767 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21768 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21769 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21770 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21771 final Acceleration by2 = new Acceleration(0.0,
21772 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21773 calibrator.getInitialBiasYAsAcceleration(by2);
21774 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21775 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21776 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21777 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21778 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21779 final Acceleration bz2 = new Acceleration(0.0,
21780 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21781 calibrator.getInitialBiasZAsAcceleration(bz2);
21782 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21783 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21784 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21785 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21786 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21787 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21788 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21789 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21790 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21791 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21792 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21793 final double[] bias1 = calibrator.getInitialBias();
21794 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21795 final double[] bias2 = new double[3];
21796 calibrator.getInitialBias(bias2);
21797 assertArrayEquals(bias1, bias2, 0.0);
21798 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21799 assertEquals(b1, ba);
21800 final Matrix b2 = new Matrix(3, 1);
21801 calibrator.getInitialBiasAsMatrix(b2);
21802 assertEquals(b1, b2);
21803 final Matrix ma1 = calibrator.getInitialMa();
21804 assertEquals(ma1, new Matrix(3, 3));
21805 final Matrix ma2 = new Matrix(3, 3);
21806 calibrator.getInitialMa(ma2);
21807 assertEquals(ma1, ma2);
21808 assertSame(calibrator.getMeasurements(), measurements);
21809 assertFalse(calibrator.isCommonAxisUsed());
21810 assertSame(calibrator.getListener(), this);
21811 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21812 assertFalse(calibrator.isReady());
21813 assertFalse(calibrator.isRunning());
21814 assertNull(calibrator.getEstimatedBiases());
21815 assertFalse(calibrator.getEstimatedBiases(null));
21816 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21817 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21818 assertNull(calibrator.getEstimatedBiasFx());
21819 assertNull(calibrator.getEstimatedBiasFy());
21820 assertNull(calibrator.getEstimatedBiasFz());
21821 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21822 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21823 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21824 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21825 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21826 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21827 assertNull(calibrator.getEstimatedMa());
21828 assertNull(calibrator.getEstimatedSx());
21829 assertNull(calibrator.getEstimatedSy());
21830 assertNull(calibrator.getEstimatedSz());
21831 assertNull(calibrator.getEstimatedMxy());
21832 assertNull(calibrator.getEstimatedMxz());
21833 assertNull(calibrator.getEstimatedMyx());
21834 assertNull(calibrator.getEstimatedMyz());
21835 assertNull(calibrator.getEstimatedMzx());
21836 assertNull(calibrator.getEstimatedMzy());
21837 assertNull(calibrator.getEstimatedCovariance());
21838 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21839 assertNotNull(calibrator.getGroundTruthGravityNorm());
21840 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21841 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21842 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21843 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21844 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21845 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21846 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21847
21848
21849 final Acceleration invalidGravityNorm = new Acceleration(
21850 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21851
21852 calibrator = null;
21853 try {
21854 calibrator = new KnownGravityNormAccelerometerCalibrator(
21855 invalidGravityNorm, measurements,
21856 biasX, biasY, biasZ, this);
21857 fail("IllegalArgumentException expected but not thrown");
21858 } catch (final IllegalArgumentException ignore) {
21859 }
21860 assertNull(calibrator);
21861 }
21862
21863 @Test
21864 public void testConstructor169() throws WrongSizeException {
21865 final Matrix ba = generateBa();
21866 final double biasX = ba.getElementAtIndex(0);
21867 final double biasY = ba.getElementAtIndex(1);
21868 final double biasZ = ba.getElementAtIndex(2);
21869
21870 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21871 final double latitude = Math.toRadians(
21872 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21873 final double longitude = Math.toRadians(
21874 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21875 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21876 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21877 final NEDVelocity nedVelocity = new NEDVelocity();
21878 final ECEFPosition ecefPosition = new ECEFPosition();
21879 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21880 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21881 ecefPosition, ecefVelocity);
21882 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21883 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21884 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21885
21886 KnownGravityNormAccelerometerCalibrator calibrator =
21887 new KnownGravityNormAccelerometerCalibrator(
21888 gravityNorm, true, biasX, biasY, biasZ);
21889
21890
21891 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21892 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21893 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21894 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21895 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21896 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21897 final Acceleration bx2 = new Acceleration(0.0,
21898 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21899 calibrator.getInitialBiasXAsAcceleration(bx2);
21900 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21901 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21903 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21904 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21905 final Acceleration by2 = new Acceleration(0.0,
21906 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21907 calibrator.getInitialBiasYAsAcceleration(by2);
21908 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21909 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21910 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21911 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21912 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21913 final Acceleration bz2 = new Acceleration(0.0,
21914 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21915 calibrator.getInitialBiasZAsAcceleration(bz2);
21916 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21917 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21918 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21919 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21920 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21921 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21922 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21923 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21924 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21925 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21926 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21927 final double[] bias1 = calibrator.getInitialBias();
21928 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21929 final double[] bias2 = new double[3];
21930 calibrator.getInitialBias(bias2);
21931 assertArrayEquals(bias1, bias2, 0.0);
21932 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21933 assertEquals(b1, ba);
21934 final Matrix b2 = new Matrix(3, 1);
21935 calibrator.getInitialBiasAsMatrix(b2);
21936 assertEquals(b1, b2);
21937 final Matrix ma1 = calibrator.getInitialMa();
21938 assertEquals(ma1, new Matrix(3, 3));
21939 final Matrix ma2 = new Matrix(3, 3);
21940 calibrator.getInitialMa(ma2);
21941 assertEquals(ma1, ma2);
21942 assertNull(calibrator.getMeasurements());
21943 assertTrue(calibrator.isCommonAxisUsed());
21944 assertNull(calibrator.getListener());
21945 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21946 assertFalse(calibrator.isReady());
21947 assertFalse(calibrator.isRunning());
21948 assertNull(calibrator.getEstimatedBiases());
21949 assertFalse(calibrator.getEstimatedBiases(null));
21950 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21951 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21952 assertNull(calibrator.getEstimatedBiasFx());
21953 assertNull(calibrator.getEstimatedBiasFy());
21954 assertNull(calibrator.getEstimatedBiasFz());
21955 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21956 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21957 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21958 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21959 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21960 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21961 assertNull(calibrator.getEstimatedMa());
21962 assertNull(calibrator.getEstimatedSx());
21963 assertNull(calibrator.getEstimatedSy());
21964 assertNull(calibrator.getEstimatedSz());
21965 assertNull(calibrator.getEstimatedMxy());
21966 assertNull(calibrator.getEstimatedMxz());
21967 assertNull(calibrator.getEstimatedMyx());
21968 assertNull(calibrator.getEstimatedMyz());
21969 assertNull(calibrator.getEstimatedMzx());
21970 assertNull(calibrator.getEstimatedMzy());
21971 assertNull(calibrator.getEstimatedCovariance());
21972 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21973 assertNotNull(calibrator.getGroundTruthGravityNorm());
21974 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21975 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21976 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21977 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21978 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21979 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21980 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21981
21982
21983 final Acceleration invalidGravityNorm = new Acceleration(
21984 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21985
21986 calibrator = null;
21987 try {
21988 calibrator = new KnownGravityNormAccelerometerCalibrator(
21989 invalidGravityNorm, true, biasX, biasY, biasZ);
21990 fail("IllegalArgumentException expected but not thrown");
21991 } catch (final IllegalArgumentException ignore) {
21992 }
21993 assertNull(calibrator);
21994 }
21995
21996 @Test
21997 public void testConstructor170() throws WrongSizeException {
21998 final Matrix ba = generateBa();
21999 final double biasX = ba.getElementAtIndex(0);
22000 final double biasY = ba.getElementAtIndex(1);
22001 final double biasZ = ba.getElementAtIndex(2);
22002
22003 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22004 final double latitude = Math.toRadians(
22005 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22006 final double longitude = Math.toRadians(
22007 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22008 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22009 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22010 final NEDVelocity nedVelocity = new NEDVelocity();
22011 final ECEFPosition ecefPosition = new ECEFPosition();
22012 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22013 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22014 ecefPosition, ecefVelocity);
22015 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22016 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22017 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22018
22019 KnownGravityNormAccelerometerCalibrator calibrator =
22020 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22021 true, biasX, biasY, biasZ, this);
22022
22023
22024 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22025 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22026 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22027 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22028 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22029 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22030 final Acceleration bx2 = new Acceleration(0.0,
22031 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22032 calibrator.getInitialBiasXAsAcceleration(bx2);
22033 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22034 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22035 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22036 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22037 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22038 final Acceleration by2 = new Acceleration(0.0,
22039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22040 calibrator.getInitialBiasYAsAcceleration(by2);
22041 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22042 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22043 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22044 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22045 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046 final Acceleration bz2 = new Acceleration(0.0,
22047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22048 calibrator.getInitialBiasZAsAcceleration(bz2);
22049 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22050 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22051 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22052 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22053 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22054 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22055 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22056 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22057 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22058 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22059 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22060 final double[] bias1 = calibrator.getInitialBias();
22061 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22062 final double[] bias2 = new double[3];
22063 calibrator.getInitialBias(bias2);
22064 assertArrayEquals(bias1, bias2, 0.0);
22065 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22066 assertEquals(b1, ba);
22067 final Matrix b2 = new Matrix(3, 1);
22068 calibrator.getInitialBiasAsMatrix(b2);
22069 assertEquals(b1, b2);
22070 final Matrix ma1 = calibrator.getInitialMa();
22071 assertEquals(ma1, new Matrix(3, 3));
22072 final Matrix ma2 = new Matrix(3, 3);
22073 calibrator.getInitialMa(ma2);
22074 assertEquals(ma1, ma2);
22075 assertNull(calibrator.getMeasurements());
22076 assertTrue(calibrator.isCommonAxisUsed());
22077 assertSame(calibrator.getListener(), this);
22078 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22079 assertFalse(calibrator.isReady());
22080 assertFalse(calibrator.isRunning());
22081 assertNull(calibrator.getEstimatedBiases());
22082 assertFalse(calibrator.getEstimatedBiases(null));
22083 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22084 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22085 assertNull(calibrator.getEstimatedBiasFx());
22086 assertNull(calibrator.getEstimatedBiasFy());
22087 assertNull(calibrator.getEstimatedBiasFz());
22088 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22089 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22090 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22091 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22092 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22093 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22094 assertNull(calibrator.getEstimatedMa());
22095 assertNull(calibrator.getEstimatedSx());
22096 assertNull(calibrator.getEstimatedSy());
22097 assertNull(calibrator.getEstimatedSz());
22098 assertNull(calibrator.getEstimatedMxy());
22099 assertNull(calibrator.getEstimatedMxz());
22100 assertNull(calibrator.getEstimatedMyx());
22101 assertNull(calibrator.getEstimatedMyz());
22102 assertNull(calibrator.getEstimatedMzx());
22103 assertNull(calibrator.getEstimatedMzy());
22104 assertNull(calibrator.getEstimatedCovariance());
22105 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22106 assertNotNull(calibrator.getGroundTruthGravityNorm());
22107 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22108 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22109 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22110 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22111 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22112 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22113 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22114
22115
22116 final Acceleration invalidGravityNorm = new Acceleration(
22117 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22118
22119 calibrator = null;
22120 try {
22121 calibrator = new KnownGravityNormAccelerometerCalibrator(
22122 invalidGravityNorm, true,
22123 biasX, biasY, biasZ, this);
22124 fail("IllegalArgumentException expected but not thrown");
22125 } catch (final IllegalArgumentException ignore) {
22126 }
22127 assertNull(calibrator);
22128 }
22129
22130 @Test
22131 public void testConstructor171() throws WrongSizeException {
22132 final Collection<StandardDeviationBodyKinematics> measurements =
22133 Collections.emptyList();
22134
22135 final Matrix ba = generateBa();
22136 final double biasX = ba.getElementAtIndex(0);
22137 final double biasY = ba.getElementAtIndex(1);
22138 final double biasZ = ba.getElementAtIndex(2);
22139
22140 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22141 final double latitude = Math.toRadians(
22142 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22143 final double longitude = Math.toRadians(
22144 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22145 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22146 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22147 final NEDVelocity nedVelocity = new NEDVelocity();
22148 final ECEFPosition ecefPosition = new ECEFPosition();
22149 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22150 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22151 ecefPosition, ecefVelocity);
22152 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22153 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22154 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22155
22156 KnownGravityNormAccelerometerCalibrator calibrator =
22157 new KnownGravityNormAccelerometerCalibrator(
22158 gravityNorm, measurements,
22159 true, biasX, biasY, biasZ);
22160
22161
22162 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22163 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22164 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22165 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22166 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22167 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22168 final Acceleration bx2 = new Acceleration(0.0,
22169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22170 calibrator.getInitialBiasXAsAcceleration(bx2);
22171 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22172 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22173 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22174 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22175 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22176 final Acceleration by2 = new Acceleration(0.0,
22177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22178 calibrator.getInitialBiasYAsAcceleration(by2);
22179 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22180 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22181 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22182 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22183 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22184 final Acceleration bz2 = new Acceleration(0.0,
22185 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22186 calibrator.getInitialBiasZAsAcceleration(bz2);
22187 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22188 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22189 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22190 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22191 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22192 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22193 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22194 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22195 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22196 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22197 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22198 final double[] bias1 = calibrator.getInitialBias();
22199 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22200 final double[] bias2 = new double[3];
22201 calibrator.getInitialBias(bias2);
22202 assertArrayEquals(bias1, bias2, 0.0);
22203 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22204 assertEquals(b1, ba);
22205 final Matrix b2 = new Matrix(3, 1);
22206 calibrator.getInitialBiasAsMatrix(b2);
22207 assertEquals(b1, b2);
22208 final Matrix ma1 = calibrator.getInitialMa();
22209 assertEquals(ma1, new Matrix(3, 3));
22210 final Matrix ma2 = new Matrix(3, 3);
22211 calibrator.getInitialMa(ma2);
22212 assertEquals(ma1, ma2);
22213 assertSame(calibrator.getMeasurements(), measurements);
22214 assertTrue(calibrator.isCommonAxisUsed());
22215 assertNull(calibrator.getListener());
22216 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22217 assertFalse(calibrator.isReady());
22218 assertFalse(calibrator.isRunning());
22219 assertNull(calibrator.getEstimatedBiases());
22220 assertFalse(calibrator.getEstimatedBiases(null));
22221 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22222 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22223 assertNull(calibrator.getEstimatedBiasFx());
22224 assertNull(calibrator.getEstimatedBiasFy());
22225 assertNull(calibrator.getEstimatedBiasFz());
22226 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22227 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22228 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22229 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22230 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22231 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22232 assertNull(calibrator.getEstimatedMa());
22233 assertNull(calibrator.getEstimatedSx());
22234 assertNull(calibrator.getEstimatedSy());
22235 assertNull(calibrator.getEstimatedSz());
22236 assertNull(calibrator.getEstimatedMxy());
22237 assertNull(calibrator.getEstimatedMxz());
22238 assertNull(calibrator.getEstimatedMyx());
22239 assertNull(calibrator.getEstimatedMyz());
22240 assertNull(calibrator.getEstimatedMzx());
22241 assertNull(calibrator.getEstimatedMzy());
22242 assertNull(calibrator.getEstimatedCovariance());
22243 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22244 assertNotNull(calibrator.getGroundTruthGravityNorm());
22245 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22246 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22247 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22248 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22249 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22250 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22251 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22252
22253
22254 final Acceleration invalidGravityNorm = new Acceleration(
22255 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22256
22257 calibrator = null;
22258 try {
22259 calibrator = new KnownGravityNormAccelerometerCalibrator(
22260 invalidGravityNorm, measurements,
22261 true, biasX, biasY, biasZ);
22262 fail("IllegalArgumentException expected but not thrown");
22263 } catch (final IllegalArgumentException ignore) {
22264 }
22265 assertNull(calibrator);
22266 }
22267
22268 @Test
22269 public void testConstructor172() throws WrongSizeException {
22270 final Collection<StandardDeviationBodyKinematics> measurements =
22271 Collections.emptyList();
22272
22273 final Matrix ba = generateBa();
22274 final double biasX = ba.getElementAtIndex(0);
22275 final double biasY = ba.getElementAtIndex(1);
22276 final double biasZ = ba.getElementAtIndex(2);
22277
22278 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22279 final double latitude = Math.toRadians(
22280 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22281 final double longitude = Math.toRadians(
22282 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22283 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22284 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22285 final NEDVelocity nedVelocity = new NEDVelocity();
22286 final ECEFPosition ecefPosition = new ECEFPosition();
22287 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22288 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22289 ecefPosition, ecefVelocity);
22290 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22291 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22292 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22293
22294 KnownGravityNormAccelerometerCalibrator calibrator =
22295 new KnownGravityNormAccelerometerCalibrator(
22296 gravityNorm, measurements,
22297 true, biasX, biasY, biasZ, this);
22298
22299
22300 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22301 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22302 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22303 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22304 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22305 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22306 final Acceleration bx2 = new Acceleration(0.0,
22307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22308 calibrator.getInitialBiasXAsAcceleration(bx2);
22309 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22310 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22311 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22312 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22313 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22314 final Acceleration by2 = new Acceleration(0.0,
22315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22316 calibrator.getInitialBiasYAsAcceleration(by2);
22317 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22318 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22319 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22320 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22321 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22322 final Acceleration bz2 = new Acceleration(0.0,
22323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22324 calibrator.getInitialBiasZAsAcceleration(bz2);
22325 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22326 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22327 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22328 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22329 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22330 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22331 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22332 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22333 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22334 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22335 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22336 final double[] bias1 = calibrator.getInitialBias();
22337 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22338 final double[] bias2 = new double[3];
22339 calibrator.getInitialBias(bias2);
22340 assertArrayEquals(bias1, bias2, 0.0);
22341 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22342 assertEquals(b1, ba);
22343 final Matrix b2 = new Matrix(3, 1);
22344 calibrator.getInitialBiasAsMatrix(b2);
22345 assertEquals(b1, b2);
22346 final Matrix ma1 = calibrator.getInitialMa();
22347 assertEquals(ma1, new Matrix(3, 3));
22348 final Matrix ma2 = new Matrix(3, 3);
22349 calibrator.getInitialMa(ma2);
22350 assertEquals(ma1, ma2);
22351 assertSame(calibrator.getMeasurements(), measurements);
22352 assertTrue(calibrator.isCommonAxisUsed());
22353 assertSame(calibrator.getListener(), this);
22354 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22355 assertFalse(calibrator.isReady());
22356 assertFalse(calibrator.isRunning());
22357 assertNull(calibrator.getEstimatedBiases());
22358 assertFalse(calibrator.getEstimatedBiases(null));
22359 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22360 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22361 assertNull(calibrator.getEstimatedBiasFx());
22362 assertNull(calibrator.getEstimatedBiasFy());
22363 assertNull(calibrator.getEstimatedBiasFz());
22364 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22365 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22366 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22367 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22368 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22369 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22370 assertNull(calibrator.getEstimatedMa());
22371 assertNull(calibrator.getEstimatedSx());
22372 assertNull(calibrator.getEstimatedSy());
22373 assertNull(calibrator.getEstimatedSz());
22374 assertNull(calibrator.getEstimatedMxy());
22375 assertNull(calibrator.getEstimatedMxz());
22376 assertNull(calibrator.getEstimatedMyx());
22377 assertNull(calibrator.getEstimatedMyz());
22378 assertNull(calibrator.getEstimatedMzx());
22379 assertNull(calibrator.getEstimatedMzy());
22380 assertNull(calibrator.getEstimatedCovariance());
22381 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22382 assertNotNull(calibrator.getGroundTruthGravityNorm());
22383 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22384 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22385 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22386 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22387 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22388 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22389 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22390
22391
22392 final Acceleration invalidGravityNorm = new Acceleration(
22393 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22394
22395 calibrator = null;
22396 try {
22397 calibrator = new KnownGravityNormAccelerometerCalibrator(
22398 invalidGravityNorm, measurements,
22399 true, biasX, biasY, biasZ, this);
22400 fail("IllegalArgumentException expected but not thrown");
22401 } catch (final IllegalArgumentException ignore) {
22402 }
22403 assertNull(calibrator);
22404 }
22405
22406 @Test
22407 public void testConstructor173() throws WrongSizeException {
22408 final Matrix ba = generateBa();
22409 final double biasX = ba.getElementAtIndex(0);
22410 final double biasY = ba.getElementAtIndex(1);
22411 final double biasZ = ba.getElementAtIndex(2);
22412
22413 final Acceleration bx = new Acceleration(biasX,
22414 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22415 final Acceleration by = new Acceleration(biasY,
22416 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22417 final Acceleration bz = new Acceleration(biasZ,
22418 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22419
22420 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22421 final double latitude = Math.toRadians(
22422 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22423 final double longitude = Math.toRadians(
22424 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22425 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22426 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22427 final NEDVelocity nedVelocity = new NEDVelocity();
22428 final ECEFPosition ecefPosition = new ECEFPosition();
22429 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22430 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22431 ecefPosition, ecefVelocity);
22432 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22433 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22434 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22435
22436 KnownGravityNormAccelerometerCalibrator calibrator =
22437 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22438 bx, by, bz);
22439
22440
22441 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22442 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22443 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22444 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22445 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22446 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22447 final Acceleration bx2 = new Acceleration(0.0,
22448 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22449 calibrator.getInitialBiasXAsAcceleration(bx2);
22450 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22451 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22452 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22453 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22454 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22455 final Acceleration by2 = new Acceleration(0.0,
22456 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22457 calibrator.getInitialBiasYAsAcceleration(by2);
22458 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22459 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22460 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22461 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22462 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22463 final Acceleration bz2 = new Acceleration(0.0,
22464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22465 calibrator.getInitialBiasZAsAcceleration(bz2);
22466 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22467 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22468 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22469 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22470 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22471 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22472 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22473 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22474 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22475 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22476 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22477 final double[] bias1 = calibrator.getInitialBias();
22478 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22479 final double[] bias2 = new double[3];
22480 calibrator.getInitialBias(bias2);
22481 assertArrayEquals(bias1, bias2, 0.0);
22482 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22483 assertEquals(b1, ba);
22484 final Matrix b2 = new Matrix(3, 1);
22485 calibrator.getInitialBiasAsMatrix(b2);
22486 assertEquals(b1, b2);
22487 final Matrix ma1 = calibrator.getInitialMa();
22488 assertEquals(ma1, new Matrix(3, 3));
22489 final Matrix ma2 = new Matrix(3, 3);
22490 calibrator.getInitialMa(ma2);
22491 assertEquals(ma1, ma2);
22492 assertNull(calibrator.getMeasurements());
22493 assertFalse(calibrator.isCommonAxisUsed());
22494 assertNull(calibrator.getListener());
22495 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22496 assertFalse(calibrator.isReady());
22497 assertFalse(calibrator.isRunning());
22498 assertNull(calibrator.getEstimatedBiases());
22499 assertFalse(calibrator.getEstimatedBiases(null));
22500 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22501 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22502 assertNull(calibrator.getEstimatedBiasFx());
22503 assertNull(calibrator.getEstimatedBiasFy());
22504 assertNull(calibrator.getEstimatedBiasFz());
22505 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22506 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22507 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22508 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22509 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22510 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22511 assertNull(calibrator.getEstimatedMa());
22512 assertNull(calibrator.getEstimatedSx());
22513 assertNull(calibrator.getEstimatedSy());
22514 assertNull(calibrator.getEstimatedSz());
22515 assertNull(calibrator.getEstimatedMxy());
22516 assertNull(calibrator.getEstimatedMxz());
22517 assertNull(calibrator.getEstimatedMyx());
22518 assertNull(calibrator.getEstimatedMyz());
22519 assertNull(calibrator.getEstimatedMzx());
22520 assertNull(calibrator.getEstimatedMzy());
22521 assertNull(calibrator.getEstimatedCovariance());
22522 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22523 assertNotNull(calibrator.getGroundTruthGravityNorm());
22524 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22525 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22526 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22527 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22528 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22529 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22530 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22531
22532
22533 final Acceleration invalidGravityNorm = new Acceleration(
22534 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22535
22536 calibrator = null;
22537 try {
22538 calibrator = new KnownGravityNormAccelerometerCalibrator(
22539 invalidGravityNorm, bx, by, bz);
22540 fail("IllegalArgumentException expected but not thrown");
22541 } catch (final IllegalArgumentException ignore) {
22542 }
22543 assertNull(calibrator);
22544 }
22545
22546 @Test
22547 public void testConstructor174() throws WrongSizeException {
22548 final Matrix ba = generateBa();
22549 final double biasX = ba.getElementAtIndex(0);
22550 final double biasY = ba.getElementAtIndex(1);
22551 final double biasZ = ba.getElementAtIndex(2);
22552
22553 final Acceleration bx = new Acceleration(biasX,
22554 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22555 final Acceleration by = new Acceleration(biasY,
22556 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22557 final Acceleration bz = new Acceleration(biasZ,
22558 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22559
22560 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22561 final double latitude = Math.toRadians(
22562 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22563 final double longitude = Math.toRadians(
22564 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22565 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22566 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22567 final NEDVelocity nedVelocity = new NEDVelocity();
22568 final ECEFPosition ecefPosition = new ECEFPosition();
22569 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22570 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22571 ecefPosition, ecefVelocity);
22572 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22573 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22574 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22575
22576 KnownGravityNormAccelerometerCalibrator calibrator =
22577 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22578 bx, by, bz, this);
22579
22580
22581 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22582 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22583 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22584 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22585 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22586 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22587 final Acceleration bx2 = new Acceleration(0.0,
22588 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22589 calibrator.getInitialBiasXAsAcceleration(bx2);
22590 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22591 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22592 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22593 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22594 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22595 final Acceleration by2 = new Acceleration(0.0,
22596 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22597 calibrator.getInitialBiasYAsAcceleration(by2);
22598 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22599 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22600 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22601 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22602 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22603 final Acceleration bz2 = new Acceleration(0.0,
22604 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22605 calibrator.getInitialBiasZAsAcceleration(bz2);
22606 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22607 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22608 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22609 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22610 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22611 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22612 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22613 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22614 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22615 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22616 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22617 final double[] bias1 = calibrator.getInitialBias();
22618 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22619 final double[] bias2 = new double[3];
22620 calibrator.getInitialBias(bias2);
22621 assertArrayEquals(bias1, bias2, 0.0);
22622 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22623 assertEquals(b1, ba);
22624 final Matrix b2 = new Matrix(3, 1);
22625 calibrator.getInitialBiasAsMatrix(b2);
22626 assertEquals(b1, b2);
22627 final Matrix ma1 = calibrator.getInitialMa();
22628 assertEquals(ma1, new Matrix(3, 3));
22629 final Matrix ma2 = new Matrix(3, 3);
22630 calibrator.getInitialMa(ma2);
22631 assertEquals(ma1, ma2);
22632 assertNull(calibrator.getMeasurements());
22633 assertFalse(calibrator.isCommonAxisUsed());
22634 assertSame(calibrator.getListener(), this);
22635 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22636 assertFalse(calibrator.isReady());
22637 assertFalse(calibrator.isRunning());
22638 assertNull(calibrator.getEstimatedBiases());
22639 assertFalse(calibrator.getEstimatedBiases(null));
22640 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22641 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22642 assertNull(calibrator.getEstimatedBiasFx());
22643 assertNull(calibrator.getEstimatedBiasFy());
22644 assertNull(calibrator.getEstimatedBiasFz());
22645 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22646 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22647 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22648 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22649 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22650 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22651 assertNull(calibrator.getEstimatedMa());
22652 assertNull(calibrator.getEstimatedSx());
22653 assertNull(calibrator.getEstimatedSy());
22654 assertNull(calibrator.getEstimatedSz());
22655 assertNull(calibrator.getEstimatedMxy());
22656 assertNull(calibrator.getEstimatedMxz());
22657 assertNull(calibrator.getEstimatedMyx());
22658 assertNull(calibrator.getEstimatedMyz());
22659 assertNull(calibrator.getEstimatedMzx());
22660 assertNull(calibrator.getEstimatedMzy());
22661 assertNull(calibrator.getEstimatedCovariance());
22662 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22663 assertNotNull(calibrator.getGroundTruthGravityNorm());
22664 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22665 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22666 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22667 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22668 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22669 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22670 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22671
22672
22673 final Acceleration invalidGravityNorm = new Acceleration(
22674 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22675
22676 calibrator = null;
22677 try {
22678 calibrator = new KnownGravityNormAccelerometerCalibrator(
22679 invalidGravityNorm, bx, by, bz, this);
22680 fail("IllegalArgumentException expected but not thrown");
22681 } catch (final IllegalArgumentException ignore) {
22682 }
22683 assertNull(calibrator);
22684 }
22685
22686 @Test
22687 public void testConstructor175() throws WrongSizeException {
22688 final Collection<StandardDeviationBodyKinematics> measurements =
22689 Collections.emptyList();
22690
22691 final Matrix ba = generateBa();
22692 final double biasX = ba.getElementAtIndex(0);
22693 final double biasY = ba.getElementAtIndex(1);
22694 final double biasZ = ba.getElementAtIndex(2);
22695
22696 final Acceleration bx = new Acceleration(biasX,
22697 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22698 final Acceleration by = new Acceleration(biasY,
22699 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22700 final Acceleration bz = new Acceleration(biasZ,
22701 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22702
22703 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22704 final double latitude = Math.toRadians(
22705 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22706 final double longitude = Math.toRadians(
22707 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22708 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22709 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22710 final NEDVelocity nedVelocity = new NEDVelocity();
22711 final ECEFPosition ecefPosition = new ECEFPosition();
22712 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22713 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22714 ecefPosition, ecefVelocity);
22715 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22716 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22717 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22718
22719 KnownGravityNormAccelerometerCalibrator calibrator =
22720 new KnownGravityNormAccelerometerCalibrator(
22721 gravityNorm, measurements, bx, by, bz);
22722
22723
22724 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22725 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22726 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22727 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22728 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22729 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22730 final Acceleration bx2 = new Acceleration(0.0,
22731 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22732 calibrator.getInitialBiasXAsAcceleration(bx2);
22733 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22734 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22735 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22736 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22737 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22738 final Acceleration by2 = new Acceleration(0.0,
22739 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22740 calibrator.getInitialBiasYAsAcceleration(by2);
22741 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22742 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22744 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22745 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22746 final Acceleration bz2 = new Acceleration(0.0,
22747 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22748 calibrator.getInitialBiasZAsAcceleration(bz2);
22749 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22750 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22751 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22752 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22753 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22754 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22755 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22756 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22757 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22758 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22759 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22760 final double[] bias1 = calibrator.getInitialBias();
22761 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22762 final double[] bias2 = new double[3];
22763 calibrator.getInitialBias(bias2);
22764 assertArrayEquals(bias1, bias2, 0.0);
22765 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22766 assertEquals(b1, ba);
22767 final Matrix b2 = new Matrix(3, 1);
22768 calibrator.getInitialBiasAsMatrix(b2);
22769 assertEquals(b1, b2);
22770 final Matrix ma1 = calibrator.getInitialMa();
22771 assertEquals(ma1, new Matrix(3, 3));
22772 final Matrix ma2 = new Matrix(3, 3);
22773 calibrator.getInitialMa(ma2);
22774 assertEquals(ma1, ma2);
22775 assertSame(calibrator.getMeasurements(), measurements);
22776 assertFalse(calibrator.isCommonAxisUsed());
22777 assertNull(calibrator.getListener());
22778 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22779 assertFalse(calibrator.isReady());
22780 assertFalse(calibrator.isRunning());
22781 assertNull(calibrator.getEstimatedBiases());
22782 assertFalse(calibrator.getEstimatedBiases(null));
22783 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22784 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22785 assertNull(calibrator.getEstimatedBiasFx());
22786 assertNull(calibrator.getEstimatedBiasFy());
22787 assertNull(calibrator.getEstimatedBiasFz());
22788 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22789 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22790 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22791 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22792 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22793 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22794 assertNull(calibrator.getEstimatedMa());
22795 assertNull(calibrator.getEstimatedSx());
22796 assertNull(calibrator.getEstimatedSy());
22797 assertNull(calibrator.getEstimatedSz());
22798 assertNull(calibrator.getEstimatedMxy());
22799 assertNull(calibrator.getEstimatedMxz());
22800 assertNull(calibrator.getEstimatedMyx());
22801 assertNull(calibrator.getEstimatedMyz());
22802 assertNull(calibrator.getEstimatedMzx());
22803 assertNull(calibrator.getEstimatedMzy());
22804 assertNull(calibrator.getEstimatedCovariance());
22805 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22806 assertNotNull(calibrator.getGroundTruthGravityNorm());
22807 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22808 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22809 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22810 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22811 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22812 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22813 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22814
22815
22816 final Acceleration invalidGravityNorm = new Acceleration(
22817 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22818
22819 calibrator = null;
22820 try {
22821 calibrator = new KnownGravityNormAccelerometerCalibrator(
22822 invalidGravityNorm, measurements, bx, by, bz);
22823 fail("IllegalArgumentException expected but not thrown");
22824 } catch (final IllegalArgumentException ignore) {
22825 }
22826 assertNull(calibrator);
22827 }
22828
22829 @Test
22830 public void testConstructor176() throws WrongSizeException {
22831 final Collection<StandardDeviationBodyKinematics> measurements =
22832 Collections.emptyList();
22833
22834 final Matrix ba = generateBa();
22835 final double biasX = ba.getElementAtIndex(0);
22836 final double biasY = ba.getElementAtIndex(1);
22837 final double biasZ = ba.getElementAtIndex(2);
22838
22839 final Acceleration bx = new Acceleration(biasX,
22840 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22841 final Acceleration by = new Acceleration(biasY,
22842 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22843 final Acceleration bz = new Acceleration(biasZ,
22844 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22845
22846 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22847 final double latitude = Math.toRadians(
22848 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22849 final double longitude = Math.toRadians(
22850 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22851 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22852 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22853 final NEDVelocity nedVelocity = new NEDVelocity();
22854 final ECEFPosition ecefPosition = new ECEFPosition();
22855 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22856 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22857 ecefPosition, ecefVelocity);
22858 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22859 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22860 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22861
22862 KnownGravityNormAccelerometerCalibrator calibrator =
22863 new KnownGravityNormAccelerometerCalibrator(
22864 gravityNorm, measurements,
22865 bx, by, bz, this);
22866
22867
22868 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22869 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22870 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22871 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22872 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22873 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22874 final Acceleration bx2 = new Acceleration(0.0,
22875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22876 calibrator.getInitialBiasXAsAcceleration(bx2);
22877 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22878 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22879 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22880 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22881 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22882 final Acceleration by2 = new Acceleration(0.0,
22883 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22884 calibrator.getInitialBiasYAsAcceleration(by2);
22885 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22886 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22887 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22888 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22889 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22890 final Acceleration bz2 = new Acceleration(0.0,
22891 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22892 calibrator.getInitialBiasZAsAcceleration(bz2);
22893 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22894 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22895 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22896 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22897 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22898 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22899 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22900 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22901 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22902 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22903 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22904 final double[] bias1 = calibrator.getInitialBias();
22905 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22906 final double[] bias2 = new double[3];
22907 calibrator.getInitialBias(bias2);
22908 assertArrayEquals(bias1, bias2, 0.0);
22909 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22910 assertEquals(b1, ba);
22911 final Matrix b2 = new Matrix(3, 1);
22912 calibrator.getInitialBiasAsMatrix(b2);
22913 assertEquals(b1, b2);
22914 final Matrix ma1 = calibrator.getInitialMa();
22915 assertEquals(ma1, new Matrix(3, 3));
22916 final Matrix ma2 = new Matrix(3, 3);
22917 calibrator.getInitialMa(ma2);
22918 assertEquals(ma1, ma2);
22919 assertSame(calibrator.getMeasurements(), measurements);
22920 assertFalse(calibrator.isCommonAxisUsed());
22921 assertSame(calibrator.getListener(), this);
22922 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22923 assertFalse(calibrator.isReady());
22924 assertFalse(calibrator.isRunning());
22925 assertNull(calibrator.getEstimatedBiases());
22926 assertFalse(calibrator.getEstimatedBiases(null));
22927 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22928 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22929 assertNull(calibrator.getEstimatedBiasFx());
22930 assertNull(calibrator.getEstimatedBiasFy());
22931 assertNull(calibrator.getEstimatedBiasFz());
22932 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22933 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22934 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22935 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22936 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22937 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22938 assertNull(calibrator.getEstimatedMa());
22939 assertNull(calibrator.getEstimatedSx());
22940 assertNull(calibrator.getEstimatedSy());
22941 assertNull(calibrator.getEstimatedSz());
22942 assertNull(calibrator.getEstimatedMxy());
22943 assertNull(calibrator.getEstimatedMxz());
22944 assertNull(calibrator.getEstimatedMyx());
22945 assertNull(calibrator.getEstimatedMyz());
22946 assertNull(calibrator.getEstimatedMzx());
22947 assertNull(calibrator.getEstimatedMzy());
22948 assertNull(calibrator.getEstimatedCovariance());
22949 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22950 assertNotNull(calibrator.getGroundTruthGravityNorm());
22951 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22952 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22953 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22954 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22955 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22956 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22957 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22958
22959
22960 final Acceleration invalidGravityNorm = new Acceleration(
22961 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22962
22963 calibrator = null;
22964 try {
22965 calibrator = new KnownGravityNormAccelerometerCalibrator(
22966 invalidGravityNorm, measurements,
22967 bx, by, bz, this);
22968 fail("IllegalArgumentException expected but not thrown");
22969 } catch (final IllegalArgumentException ignore) {
22970 }
22971 assertNull(calibrator);
22972 }
22973
22974 @Test
22975 public void testConstructor177() throws WrongSizeException {
22976 final Matrix ba = generateBa();
22977 final double biasX = ba.getElementAtIndex(0);
22978 final double biasY = ba.getElementAtIndex(1);
22979 final double biasZ = ba.getElementAtIndex(2);
22980
22981 final Acceleration bx = new Acceleration(biasX,
22982 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22983 final Acceleration by = new Acceleration(biasY,
22984 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22985 final Acceleration bz = new Acceleration(biasZ,
22986 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22987
22988 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22989 final double latitude = Math.toRadians(
22990 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22991 final double longitude = Math.toRadians(
22992 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22993 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22994 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22995 final NEDVelocity nedVelocity = new NEDVelocity();
22996 final ECEFPosition ecefPosition = new ECEFPosition();
22997 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22998 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22999 ecefPosition, ecefVelocity);
23000 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23001 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23002 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23003
23004 KnownGravityNormAccelerometerCalibrator calibrator =
23005 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23006 true, bx, by, bz);
23007
23008
23009 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23010 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23011 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23012 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23013 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23014 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23015 final Acceleration bx2 = new Acceleration(0.0,
23016 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23017 calibrator.getInitialBiasXAsAcceleration(bx2);
23018 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23019 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23020 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23021 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23022 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23023 final Acceleration by2 = new Acceleration(0.0,
23024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23025 calibrator.getInitialBiasYAsAcceleration(by2);
23026 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23027 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23028 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23029 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23030 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23031 final Acceleration bz2 = new Acceleration(0.0,
23032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23033 calibrator.getInitialBiasZAsAcceleration(bz2);
23034 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23035 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23036 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23037 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23038 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23039 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23040 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23041 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23042 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23043 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23044 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23045 final double[] bias1 = calibrator.getInitialBias();
23046 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23047 final double[] bias2 = new double[3];
23048 calibrator.getInitialBias(bias2);
23049 assertArrayEquals(bias1, bias2, 0.0);
23050 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23051 assertEquals(b1, ba);
23052 final Matrix b2 = new Matrix(3, 1);
23053 calibrator.getInitialBiasAsMatrix(b2);
23054 assertEquals(b1, b2);
23055 final Matrix ma1 = calibrator.getInitialMa();
23056 assertEquals(ma1, new Matrix(3, 3));
23057 final Matrix ma2 = new Matrix(3, 3);
23058 calibrator.getInitialMa(ma2);
23059 assertEquals(ma1, ma2);
23060 assertNull(calibrator.getMeasurements());
23061 assertTrue(calibrator.isCommonAxisUsed());
23062 assertNull(calibrator.getListener());
23063 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23064 assertFalse(calibrator.isReady());
23065 assertFalse(calibrator.isRunning());
23066 assertNull(calibrator.getEstimatedBiases());
23067 assertFalse(calibrator.getEstimatedBiases(null));
23068 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23069 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23070 assertNull(calibrator.getEstimatedBiasFx());
23071 assertNull(calibrator.getEstimatedBiasFy());
23072 assertNull(calibrator.getEstimatedBiasFz());
23073 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23074 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23075 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23076 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23077 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23078 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23079 assertNull(calibrator.getEstimatedMa());
23080 assertNull(calibrator.getEstimatedSx());
23081 assertNull(calibrator.getEstimatedSy());
23082 assertNull(calibrator.getEstimatedSz());
23083 assertNull(calibrator.getEstimatedMxy());
23084 assertNull(calibrator.getEstimatedMxz());
23085 assertNull(calibrator.getEstimatedMyx());
23086 assertNull(calibrator.getEstimatedMyz());
23087 assertNull(calibrator.getEstimatedMzx());
23088 assertNull(calibrator.getEstimatedMzy());
23089 assertNull(calibrator.getEstimatedCovariance());
23090 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23091 assertNotNull(calibrator.getGroundTruthGravityNorm());
23092 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23093 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23094 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23095 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23096 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23097 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23098 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23099
23100
23101 final Acceleration invalidGravityNorm = new Acceleration(
23102 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23103
23104 calibrator = null;
23105 try {
23106 calibrator = new KnownGravityNormAccelerometerCalibrator(
23107 invalidGravityNorm, true,
23108 bx, by, bz);
23109 fail("IllegalArgumentException expected but not thrown");
23110 } catch (final IllegalArgumentException ignore) {
23111 }
23112 assertNull(calibrator);
23113 }
23114
23115 @Test
23116 public void testConstructor178() throws WrongSizeException {
23117 final Matrix ba = generateBa();
23118 final double biasX = ba.getElementAtIndex(0);
23119 final double biasY = ba.getElementAtIndex(1);
23120 final double biasZ = ba.getElementAtIndex(2);
23121
23122 final Acceleration bx = new Acceleration(biasX,
23123 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23124 final Acceleration by = new Acceleration(biasY,
23125 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23126 final Acceleration bz = new Acceleration(biasZ,
23127 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23128
23129 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23130 final double latitude = Math.toRadians(
23131 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23132 final double longitude = Math.toRadians(
23133 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23134 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23135 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23136 final NEDVelocity nedVelocity = new NEDVelocity();
23137 final ECEFPosition ecefPosition = new ECEFPosition();
23138 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23139 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23140 ecefPosition, ecefVelocity);
23141 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23142 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23143 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23144
23145 KnownGravityNormAccelerometerCalibrator calibrator =
23146 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23147 true, bx, by, bz, this);
23148
23149
23150 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23151 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23152 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23153 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23154 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23155 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23156 final Acceleration bx2 = new Acceleration(0.0,
23157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23158 calibrator.getInitialBiasXAsAcceleration(bx2);
23159 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23160 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23161 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23162 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23163 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23164 final Acceleration by2 = new Acceleration(0.0,
23165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23166 calibrator.getInitialBiasYAsAcceleration(by2);
23167 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23168 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23169 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23170 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23171 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23172 final Acceleration bz2 = new Acceleration(0.0,
23173 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23174 calibrator.getInitialBiasZAsAcceleration(bz2);
23175 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23176 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23177 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23178 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23179 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23180 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23181 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23182 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23183 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23184 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23185 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23186 final double[] bias1 = calibrator.getInitialBias();
23187 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23188 final double[] bias2 = new double[3];
23189 calibrator.getInitialBias(bias2);
23190 assertArrayEquals(bias1, bias2, 0.0);
23191 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23192 assertEquals(b1, ba);
23193 final Matrix b2 = new Matrix(3, 1);
23194 calibrator.getInitialBiasAsMatrix(b2);
23195 assertEquals(b1, b2);
23196 final Matrix ma1 = calibrator.getInitialMa();
23197 assertEquals(ma1, new Matrix(3, 3));
23198 final Matrix ma2 = new Matrix(3, 3);
23199 calibrator.getInitialMa(ma2);
23200 assertEquals(ma1, ma2);
23201 assertNull(calibrator.getMeasurements());
23202 assertTrue(calibrator.isCommonAxisUsed());
23203 assertSame(calibrator.getListener(), this);
23204 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23205 assertFalse(calibrator.isReady());
23206 assertFalse(calibrator.isRunning());
23207 assertNull(calibrator.getEstimatedBiases());
23208 assertFalse(calibrator.getEstimatedBiases(null));
23209 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23210 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23211 assertNull(calibrator.getEstimatedBiasFx());
23212 assertNull(calibrator.getEstimatedBiasFy());
23213 assertNull(calibrator.getEstimatedBiasFz());
23214 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23215 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23216 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23217 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23218 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23219 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23220 assertNull(calibrator.getEstimatedMa());
23221 assertNull(calibrator.getEstimatedSx());
23222 assertNull(calibrator.getEstimatedSy());
23223 assertNull(calibrator.getEstimatedSz());
23224 assertNull(calibrator.getEstimatedMxy());
23225 assertNull(calibrator.getEstimatedMxz());
23226 assertNull(calibrator.getEstimatedMyx());
23227 assertNull(calibrator.getEstimatedMyz());
23228 assertNull(calibrator.getEstimatedMzx());
23229 assertNull(calibrator.getEstimatedMzy());
23230 assertNull(calibrator.getEstimatedCovariance());
23231 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23232 assertNotNull(calibrator.getGroundTruthGravityNorm());
23233 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23234 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23235 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23236 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23237 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23238 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23239 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23240
23241
23242 final Acceleration invalidGravityNorm = new Acceleration(
23243 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23244
23245 calibrator = null;
23246 try {
23247 calibrator = new KnownGravityNormAccelerometerCalibrator(
23248 invalidGravityNorm, true,
23249 bx, by, bz, this);
23250 fail("IllegalArgumentException expected but not thrown");
23251 } catch (final IllegalArgumentException ignore) {
23252 }
23253 assertNull(calibrator);
23254 }
23255
23256 @Test
23257 public void testConstructor179() throws WrongSizeException {
23258 final Collection<StandardDeviationBodyKinematics> measurements =
23259 Collections.emptyList();
23260
23261 final Matrix ba = generateBa();
23262 final double biasX = ba.getElementAtIndex(0);
23263 final double biasY = ba.getElementAtIndex(1);
23264 final double biasZ = ba.getElementAtIndex(2);
23265
23266 final Acceleration bx = new Acceleration(biasX,
23267 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23268 final Acceleration by = new Acceleration(biasY,
23269 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23270 final Acceleration bz = new Acceleration(biasZ,
23271 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23272
23273 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23274 final double latitude = Math.toRadians(
23275 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23276 final double longitude = Math.toRadians(
23277 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23278 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23279 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23280 final NEDVelocity nedVelocity = new NEDVelocity();
23281 final ECEFPosition ecefPosition = new ECEFPosition();
23282 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23283 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23284 ecefPosition, ecefVelocity);
23285 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23286 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23287 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23288
23289 KnownGravityNormAccelerometerCalibrator calibrator =
23290 new KnownGravityNormAccelerometerCalibrator(
23291 gravityNorm, measurements,
23292 true, bx, by, bz);
23293
23294
23295 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23296 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23297 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23298 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23299 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23300 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23301 final Acceleration bx2 = new Acceleration(0.0,
23302 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23303 calibrator.getInitialBiasXAsAcceleration(bx2);
23304 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23305 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23306 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23307 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23308 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23309 final Acceleration by2 = new Acceleration(0.0,
23310 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23311 calibrator.getInitialBiasYAsAcceleration(by2);
23312 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23313 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23314 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23315 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23316 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23317 final Acceleration bz2 = new Acceleration(0.0,
23318 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23319 calibrator.getInitialBiasZAsAcceleration(bz2);
23320 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23321 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23322 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23323 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23324 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23325 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23326 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23327 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23328 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23329 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23330 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23331 final double[] bias1 = calibrator.getInitialBias();
23332 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23333 final double[] bias2 = new double[3];
23334 calibrator.getInitialBias(bias2);
23335 assertArrayEquals(bias1, bias2, 0.0);
23336 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23337 assertEquals(b1, ba);
23338 final Matrix b2 = new Matrix(3, 1);
23339 calibrator.getInitialBiasAsMatrix(b2);
23340 assertEquals(b1, b2);
23341 final Matrix ma1 = calibrator.getInitialMa();
23342 assertEquals(ma1, new Matrix(3, 3));
23343 final Matrix ma2 = new Matrix(3, 3);
23344 calibrator.getInitialMa(ma2);
23345 assertEquals(ma1, ma2);
23346 assertSame(calibrator.getMeasurements(), measurements);
23347 assertTrue(calibrator.isCommonAxisUsed());
23348 assertNull(calibrator.getListener());
23349 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23350 assertFalse(calibrator.isReady());
23351 assertFalse(calibrator.isRunning());
23352 assertNull(calibrator.getEstimatedBiases());
23353 assertFalse(calibrator.getEstimatedBiases(null));
23354 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23355 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23356 assertNull(calibrator.getEstimatedBiasFx());
23357 assertNull(calibrator.getEstimatedBiasFy());
23358 assertNull(calibrator.getEstimatedBiasFz());
23359 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23360 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23361 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23362 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23363 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23364 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23365 assertNull(calibrator.getEstimatedMa());
23366 assertNull(calibrator.getEstimatedSx());
23367 assertNull(calibrator.getEstimatedSy());
23368 assertNull(calibrator.getEstimatedSz());
23369 assertNull(calibrator.getEstimatedMxy());
23370 assertNull(calibrator.getEstimatedMxz());
23371 assertNull(calibrator.getEstimatedMyx());
23372 assertNull(calibrator.getEstimatedMyz());
23373 assertNull(calibrator.getEstimatedMzx());
23374 assertNull(calibrator.getEstimatedMzy());
23375 assertNull(calibrator.getEstimatedCovariance());
23376 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23377 assertNotNull(calibrator.getGroundTruthGravityNorm());
23378 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23379 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23380 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23381 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23382 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23383 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23384 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23385
23386
23387 final Acceleration invalidGravityNorm = new Acceleration(
23388 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23389
23390 calibrator = null;
23391 try {
23392 calibrator = new KnownGravityNormAccelerometerCalibrator(
23393 invalidGravityNorm, measurements,
23394 true, bx, by, bz);
23395 fail("IllegalArgumentException expected but not thrown");
23396 } catch (final IllegalArgumentException ignore) {
23397 }
23398 assertNull(calibrator);
23399 }
23400
23401 @Test
23402 public void testConstructor180() throws WrongSizeException {
23403 final Collection<StandardDeviationBodyKinematics> measurements =
23404 Collections.emptyList();
23405
23406 final Matrix ba = generateBa();
23407 final double biasX = ba.getElementAtIndex(0);
23408 final double biasY = ba.getElementAtIndex(1);
23409 final double biasZ = ba.getElementAtIndex(2);
23410
23411 final Acceleration bx = new Acceleration(biasX,
23412 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23413 final Acceleration by = new Acceleration(biasY,
23414 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23415 final Acceleration bz = new Acceleration(biasZ,
23416 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23417
23418 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23419 final double latitude = Math.toRadians(
23420 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23421 final double longitude = Math.toRadians(
23422 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23423 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23424 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23425 final NEDVelocity nedVelocity = new NEDVelocity();
23426 final ECEFPosition ecefPosition = new ECEFPosition();
23427 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23428 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23429 ecefPosition, ecefVelocity);
23430 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23431 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23432 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23433
23434 KnownGravityNormAccelerometerCalibrator calibrator =
23435 new KnownGravityNormAccelerometerCalibrator(
23436 gravityNorm, measurements,
23437 true, bx, by, bz, this);
23438
23439
23440 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23441 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23442 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23443 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23444 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23445 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23446 final Acceleration bx2 = new Acceleration(0.0,
23447 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23448 calibrator.getInitialBiasXAsAcceleration(bx2);
23449 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23450 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23451 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23452 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23453 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23454 final Acceleration by2 = new Acceleration(0.0,
23455 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23456 calibrator.getInitialBiasYAsAcceleration(by2);
23457 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23458 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23459 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23460 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23461 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23462 final Acceleration bz2 = new Acceleration(0.0,
23463 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23464 calibrator.getInitialBiasZAsAcceleration(bz2);
23465 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23466 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23467 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23468 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23469 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23470 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23471 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23472 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23473 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23474 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23475 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23476 final double[] bias1 = calibrator.getInitialBias();
23477 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23478 final double[] bias2 = new double[3];
23479 calibrator.getInitialBias(bias2);
23480 assertArrayEquals(bias1, bias2, 0.0);
23481 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23482 assertEquals(b1, ba);
23483 final Matrix b2 = new Matrix(3, 1);
23484 calibrator.getInitialBiasAsMatrix(b2);
23485 assertEquals(b1, b2);
23486 final Matrix ma1 = calibrator.getInitialMa();
23487 assertEquals(ma1, new Matrix(3, 3));
23488 final Matrix ma2 = new Matrix(3, 3);
23489 calibrator.getInitialMa(ma2);
23490 assertEquals(ma1, ma2);
23491 assertSame(calibrator.getMeasurements(), measurements);
23492 assertTrue(calibrator.isCommonAxisUsed());
23493 assertSame(calibrator.getListener(), this);
23494 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23495 assertFalse(calibrator.isReady());
23496 assertFalse(calibrator.isRunning());
23497 assertNull(calibrator.getEstimatedBiases());
23498 assertFalse(calibrator.getEstimatedBiases(null));
23499 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23500 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23501 assertNull(calibrator.getEstimatedBiasFx());
23502 assertNull(calibrator.getEstimatedBiasFy());
23503 assertNull(calibrator.getEstimatedBiasFz());
23504 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23505 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23506 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23507 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23508 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23509 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23510 assertNull(calibrator.getEstimatedMa());
23511 assertNull(calibrator.getEstimatedSx());
23512 assertNull(calibrator.getEstimatedSy());
23513 assertNull(calibrator.getEstimatedSz());
23514 assertNull(calibrator.getEstimatedMxy());
23515 assertNull(calibrator.getEstimatedMxz());
23516 assertNull(calibrator.getEstimatedMyx());
23517 assertNull(calibrator.getEstimatedMyz());
23518 assertNull(calibrator.getEstimatedMzx());
23519 assertNull(calibrator.getEstimatedMzy());
23520 assertNull(calibrator.getEstimatedCovariance());
23521 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23522 assertNotNull(calibrator.getGroundTruthGravityNorm());
23523 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23524 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23525 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23526 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23527 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23528 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23529 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23530
23531
23532 final Acceleration invalidGravityNorm = new Acceleration(
23533 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23534
23535 calibrator = null;
23536 try {
23537 calibrator = new KnownGravityNormAccelerometerCalibrator(
23538 invalidGravityNorm, measurements, true,
23539 bx, by, bz, this);
23540 fail("IllegalArgumentException expected but not thrown");
23541 } catch (final IllegalArgumentException ignore) {
23542 }
23543 assertNull(calibrator);
23544 }
23545
23546 @Test
23547 public void testConstructor181() throws WrongSizeException {
23548 final Matrix ba = generateBa();
23549 final double biasX = ba.getElementAtIndex(0);
23550 final double biasY = ba.getElementAtIndex(1);
23551 final double biasZ = ba.getElementAtIndex(2);
23552
23553 final Matrix ma = generateMaCommonAxis();
23554 final double sx = ma.getElementAt(0, 0);
23555 final double sy = ma.getElementAt(1, 1);
23556 final double sz = ma.getElementAt(2, 2);
23557
23558 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23559 final double latitude = Math.toRadians(
23560 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23561 final double longitude = Math.toRadians(
23562 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23563 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23564 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23565 final NEDVelocity nedVelocity = new NEDVelocity();
23566 final ECEFPosition ecefPosition = new ECEFPosition();
23567 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23568 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23569 ecefPosition, ecefVelocity);
23570 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23571 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23572 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23573
23574 KnownGravityNormAccelerometerCalibrator calibrator =
23575 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23576 biasX, biasY, biasZ, sx, sy, sz);
23577
23578
23579 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23580 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23581 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23582 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23583 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23584 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23585 final Acceleration bx2 = new Acceleration(0.0,
23586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23587 calibrator.getInitialBiasXAsAcceleration(bx2);
23588 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23589 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23590 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23591 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23592 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23593 final Acceleration by2 = new Acceleration(0.0,
23594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23595 calibrator.getInitialBiasYAsAcceleration(by2);
23596 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23597 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23598 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23599 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23600 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23601 final Acceleration bz2 = new Acceleration(0.0,
23602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23603 calibrator.getInitialBiasZAsAcceleration(bz2);
23604 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23605 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23606 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23607 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23608 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23609 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23610 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23611 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23612 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23613 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23614 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23615 final double[] bias1 = calibrator.getInitialBias();
23616 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23617 final double[] bias2 = new double[3];
23618 calibrator.getInitialBias(bias2);
23619 assertArrayEquals(bias1, bias2, 0.0);
23620 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23621 assertEquals(b1, ba);
23622 final Matrix b2 = new Matrix(3, 1);
23623 calibrator.getInitialBiasAsMatrix(b2);
23624 assertEquals(b1, b2);
23625 final Matrix ma1 = calibrator.getInitialMa();
23626 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23627 final Matrix ma2 = new Matrix(3, 3);
23628 calibrator.getInitialMa(ma2);
23629 assertEquals(ma1, ma2);
23630 assertNull(calibrator.getMeasurements());
23631 assertFalse(calibrator.isCommonAxisUsed());
23632 assertNull(calibrator.getListener());
23633 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23634 assertFalse(calibrator.isReady());
23635 assertFalse(calibrator.isRunning());
23636 assertNull(calibrator.getEstimatedBiases());
23637 assertFalse(calibrator.getEstimatedBiases(null));
23638 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23639 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23640 assertNull(calibrator.getEstimatedBiasFx());
23641 assertNull(calibrator.getEstimatedBiasFy());
23642 assertNull(calibrator.getEstimatedBiasFz());
23643 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23644 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23645 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23646 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23647 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23648 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23649 assertNull(calibrator.getEstimatedMa());
23650 assertNull(calibrator.getEstimatedSx());
23651 assertNull(calibrator.getEstimatedSy());
23652 assertNull(calibrator.getEstimatedSz());
23653 assertNull(calibrator.getEstimatedMxy());
23654 assertNull(calibrator.getEstimatedMxz());
23655 assertNull(calibrator.getEstimatedMyx());
23656 assertNull(calibrator.getEstimatedMyz());
23657 assertNull(calibrator.getEstimatedMzx());
23658 assertNull(calibrator.getEstimatedMzy());
23659 assertNull(calibrator.getEstimatedCovariance());
23660 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23661 assertNotNull(calibrator.getGroundTruthGravityNorm());
23662 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23663 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23664 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23665 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23666 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23667 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23668 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23669
23670
23671 final Acceleration invalidGravityNorm = new Acceleration(
23672 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23673
23674 calibrator = null;
23675 try {
23676 calibrator = new KnownGravityNormAccelerometerCalibrator(
23677 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz);
23678 fail("IllegalArgumentException expected but not thrown");
23679 } catch (final IllegalArgumentException ignore) {
23680 }
23681 assertNull(calibrator);
23682 }
23683
23684 @Test
23685 public void testConstructor182() throws WrongSizeException {
23686 final Collection<StandardDeviationBodyKinematics> measurements =
23687 Collections.emptyList();
23688
23689 final Matrix ba = generateBa();
23690 final double biasX = ba.getElementAtIndex(0);
23691 final double biasY = ba.getElementAtIndex(1);
23692 final double biasZ = ba.getElementAtIndex(2);
23693
23694 final Matrix ma = generateMaCommonAxis();
23695 final double sx = ma.getElementAt(0, 0);
23696 final double sy = ma.getElementAt(1, 1);
23697 final double sz = ma.getElementAt(2, 2);
23698
23699 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23700 final double latitude = Math.toRadians(
23701 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23702 final double longitude = Math.toRadians(
23703 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23704 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23705 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23706 final NEDVelocity nedVelocity = new NEDVelocity();
23707 final ECEFPosition ecefPosition = new ECEFPosition();
23708 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23709 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23710 ecefPosition, ecefVelocity);
23711 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23712 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23713 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23714
23715 KnownGravityNormAccelerometerCalibrator calibrator =
23716 new KnownGravityNormAccelerometerCalibrator(
23717 gravityNorm, measurements,
23718 biasX, biasY, biasZ, sx, sy, sz);
23719
23720
23721 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23722 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23723 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23724 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23725 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23726 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23727 final Acceleration bx2 = new Acceleration(0.0,
23728 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23729 calibrator.getInitialBiasXAsAcceleration(bx2);
23730 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23731 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23732 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23733 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23734 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23735 final Acceleration by2 = new Acceleration(0.0,
23736 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23737 calibrator.getInitialBiasYAsAcceleration(by2);
23738 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23739 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23740 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23741 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23742 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23743 final Acceleration bz2 = new Acceleration(0.0,
23744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23745 calibrator.getInitialBiasZAsAcceleration(bz2);
23746 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23747 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23748 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23749 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23750 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23751 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23752 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23753 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23754 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23755 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23756 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23757 final double[] bias1 = calibrator.getInitialBias();
23758 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23759 final double[] bias2 = new double[3];
23760 calibrator.getInitialBias(bias2);
23761 assertArrayEquals(bias1, bias2, 0.0);
23762 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23763 assertEquals(b1, ba);
23764 final Matrix b2 = new Matrix(3, 1);
23765 calibrator.getInitialBiasAsMatrix(b2);
23766 assertEquals(b1, b2);
23767 final Matrix ma1 = calibrator.getInitialMa();
23768 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23769 final Matrix ma2 = new Matrix(3, 3);
23770 calibrator.getInitialMa(ma2);
23771 assertEquals(ma1, ma2);
23772 assertSame(calibrator.getMeasurements(), measurements);
23773 assertFalse(calibrator.isCommonAxisUsed());
23774 assertNull(calibrator.getListener());
23775 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23776 assertFalse(calibrator.isReady());
23777 assertFalse(calibrator.isRunning());
23778 assertNull(calibrator.getEstimatedBiases());
23779 assertFalse(calibrator.getEstimatedBiases(null));
23780 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23781 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23782 assertNull(calibrator.getEstimatedBiasFx());
23783 assertNull(calibrator.getEstimatedBiasFy());
23784 assertNull(calibrator.getEstimatedBiasFz());
23785 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23786 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23787 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23788 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23789 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23790 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23791 assertNull(calibrator.getEstimatedMa());
23792 assertNull(calibrator.getEstimatedSx());
23793 assertNull(calibrator.getEstimatedSy());
23794 assertNull(calibrator.getEstimatedSz());
23795 assertNull(calibrator.getEstimatedMxy());
23796 assertNull(calibrator.getEstimatedMxz());
23797 assertNull(calibrator.getEstimatedMyx());
23798 assertNull(calibrator.getEstimatedMyz());
23799 assertNull(calibrator.getEstimatedMzx());
23800 assertNull(calibrator.getEstimatedMzy());
23801 assertNull(calibrator.getEstimatedCovariance());
23802 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23803 assertNotNull(calibrator.getGroundTruthGravityNorm());
23804 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23805 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23806 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23807 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23808 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23809 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23810 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23811
23812
23813 final Acceleration invalidGravityNorm = new Acceleration(
23814 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23815
23816 calibrator = null;
23817 try {
23818 calibrator = new KnownGravityNormAccelerometerCalibrator(
23819 invalidGravityNorm, measurements,
23820 biasX, biasY, biasZ, sx, sy, sz);
23821 fail("IllegalArgumentException expected but not thrown");
23822 } catch (final IllegalArgumentException ignore) {
23823 }
23824 assertNull(calibrator);
23825 }
23826
23827 @Test
23828 public void testConstructor183() throws WrongSizeException {
23829 final Collection<StandardDeviationBodyKinematics> measurements =
23830 Collections.emptyList();
23831
23832 final Matrix ba = generateBa();
23833 final double biasX = ba.getElementAtIndex(0);
23834 final double biasY = ba.getElementAtIndex(1);
23835 final double biasZ = ba.getElementAtIndex(2);
23836
23837 final Matrix ma = generateMaCommonAxis();
23838 final double sx = ma.getElementAt(0, 0);
23839 final double sy = ma.getElementAt(1, 1);
23840 final double sz = ma.getElementAt(2, 2);
23841
23842 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23843 final double latitude = Math.toRadians(
23844 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23845 final double longitude = Math.toRadians(
23846 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23847 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23848 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23849 final NEDVelocity nedVelocity = new NEDVelocity();
23850 final ECEFPosition ecefPosition = new ECEFPosition();
23851 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23852 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23853 ecefPosition, ecefVelocity);
23854 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23855 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23856 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23857
23858 KnownGravityNormAccelerometerCalibrator calibrator =
23859 new KnownGravityNormAccelerometerCalibrator(
23860 gravityNorm, measurements,
23861 biasX, biasY, biasZ, sx, sy, sz, this);
23862
23863
23864 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23865 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23866 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23867 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23868 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23869 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23870 final Acceleration bx2 = new Acceleration(0.0,
23871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23872 calibrator.getInitialBiasXAsAcceleration(bx2);
23873 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23874 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23875 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23876 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23877 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23878 final Acceleration by2 = new Acceleration(0.0,
23879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23880 calibrator.getInitialBiasYAsAcceleration(by2);
23881 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23882 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23883 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23884 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23885 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23886 final Acceleration bz2 = new Acceleration(0.0,
23887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23888 calibrator.getInitialBiasZAsAcceleration(bz2);
23889 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23890 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23891 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23892 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23893 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23894 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23895 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23896 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23897 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23898 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23899 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23900 final double[] bias1 = calibrator.getInitialBias();
23901 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23902 final double[] bias2 = new double[3];
23903 calibrator.getInitialBias(bias2);
23904 assertArrayEquals(bias1, bias2, 0.0);
23905 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23906 assertEquals(b1, ba);
23907 final Matrix b2 = new Matrix(3, 1);
23908 calibrator.getInitialBiasAsMatrix(b2);
23909 assertEquals(b1, b2);
23910 final Matrix ma1 = calibrator.getInitialMa();
23911 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23912 final Matrix ma2 = new Matrix(3, 3);
23913 calibrator.getInitialMa(ma2);
23914 assertEquals(ma1, ma2);
23915 assertSame(calibrator.getMeasurements(), measurements);
23916 assertFalse(calibrator.isCommonAxisUsed());
23917 assertSame(calibrator.getListener(), this);
23918 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23919 assertFalse(calibrator.isReady());
23920 assertFalse(calibrator.isRunning());
23921 assertNull(calibrator.getEstimatedBiases());
23922 assertFalse(calibrator.getEstimatedBiases(null));
23923 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23924 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23925 assertNull(calibrator.getEstimatedBiasFx());
23926 assertNull(calibrator.getEstimatedBiasFy());
23927 assertNull(calibrator.getEstimatedBiasFz());
23928 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23929 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23930 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23931 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23932 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23933 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23934 assertNull(calibrator.getEstimatedMa());
23935 assertNull(calibrator.getEstimatedSx());
23936 assertNull(calibrator.getEstimatedSy());
23937 assertNull(calibrator.getEstimatedSz());
23938 assertNull(calibrator.getEstimatedMxy());
23939 assertNull(calibrator.getEstimatedMxz());
23940 assertNull(calibrator.getEstimatedMyx());
23941 assertNull(calibrator.getEstimatedMyz());
23942 assertNull(calibrator.getEstimatedMzx());
23943 assertNull(calibrator.getEstimatedMzy());
23944 assertNull(calibrator.getEstimatedCovariance());
23945 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23946 assertNotNull(calibrator.getGroundTruthGravityNorm());
23947 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23948 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23949 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23950 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23951 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23952 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23953 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23954
23955
23956 final Acceleration invalidGravityNorm = new Acceleration(
23957 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23958
23959 calibrator = null;
23960 try {
23961 calibrator = new KnownGravityNormAccelerometerCalibrator(
23962 invalidGravityNorm, measurements,
23963 biasX, biasY, biasZ, sx, sy, sz, this);
23964 fail("IllegalArgumentException expected but not thrown");
23965 } catch (final IllegalArgumentException ignore) {
23966 }
23967 assertNull(calibrator);
23968 }
23969
23970 @Test
23971 public void testConstructor184() throws WrongSizeException {
23972 final Matrix ba = generateBa();
23973 final double biasX = ba.getElementAtIndex(0);
23974 final double biasY = ba.getElementAtIndex(1);
23975 final double biasZ = ba.getElementAtIndex(2);
23976
23977 final Matrix ma = generateMaCommonAxis();
23978 final double sx = ma.getElementAt(0, 0);
23979 final double sy = ma.getElementAt(1, 1);
23980 final double sz = ma.getElementAt(2, 2);
23981
23982 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23983 final double latitude = Math.toRadians(
23984 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23985 final double longitude = Math.toRadians(
23986 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23987 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23988 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23989 final NEDVelocity nedVelocity = new NEDVelocity();
23990 final ECEFPosition ecefPosition = new ECEFPosition();
23991 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23992 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23993 ecefPosition, ecefVelocity);
23994 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23995 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23996 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23997
23998 KnownGravityNormAccelerometerCalibrator calibrator =
23999 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24000 true, biasX, biasY, biasZ, sx, sy, sz);
24001
24002
24003 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24004 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24005 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24006 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24007 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24008 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24009 final Acceleration bx2 = new Acceleration(0.0,
24010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24011 calibrator.getInitialBiasXAsAcceleration(bx2);
24012 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24013 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24014 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24015 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24016 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24017 final Acceleration by2 = new Acceleration(0.0,
24018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24019 calibrator.getInitialBiasYAsAcceleration(by2);
24020 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24021 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24022 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24023 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24024 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24025 final Acceleration bz2 = new Acceleration(0.0,
24026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24027 calibrator.getInitialBiasZAsAcceleration(bz2);
24028 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24029 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24030 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24031 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24032 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24033 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24034 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24035 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24036 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24037 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24038 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24039 final double[] bias1 = calibrator.getInitialBias();
24040 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24041 final double[] bias2 = new double[3];
24042 calibrator.getInitialBias(bias2);
24043 assertArrayEquals(bias1, bias2, 0.0);
24044 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24045 assertEquals(b1, ba);
24046 final Matrix b2 = new Matrix(3, 1);
24047 calibrator.getInitialBiasAsMatrix(b2);
24048 assertEquals(b1, b2);
24049 final Matrix ma1 = calibrator.getInitialMa();
24050 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24051 final Matrix ma2 = new Matrix(3, 3);
24052 calibrator.getInitialMa(ma2);
24053 assertEquals(ma1, ma2);
24054 assertNull(calibrator.getMeasurements());
24055 assertTrue(calibrator.isCommonAxisUsed());
24056 assertNull(calibrator.getListener());
24057 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24058 assertFalse(calibrator.isReady());
24059 assertFalse(calibrator.isRunning());
24060 assertNull(calibrator.getEstimatedBiases());
24061 assertFalse(calibrator.getEstimatedBiases(null));
24062 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24063 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24064 assertNull(calibrator.getEstimatedBiasFx());
24065 assertNull(calibrator.getEstimatedBiasFy());
24066 assertNull(calibrator.getEstimatedBiasFz());
24067 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24068 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24069 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24070 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24071 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24072 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24073 assertNull(calibrator.getEstimatedMa());
24074 assertNull(calibrator.getEstimatedSx());
24075 assertNull(calibrator.getEstimatedSy());
24076 assertNull(calibrator.getEstimatedSz());
24077 assertNull(calibrator.getEstimatedMxy());
24078 assertNull(calibrator.getEstimatedMxz());
24079 assertNull(calibrator.getEstimatedMyx());
24080 assertNull(calibrator.getEstimatedMyz());
24081 assertNull(calibrator.getEstimatedMzx());
24082 assertNull(calibrator.getEstimatedMzy());
24083 assertNull(calibrator.getEstimatedCovariance());
24084 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24085 assertNotNull(calibrator.getGroundTruthGravityNorm());
24086 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24087 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24088 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24089 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24090 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24091 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24092 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24093
24094
24095 final Acceleration invalidGravityNorm = new Acceleration(
24096 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24097
24098 calibrator = null;
24099 try {
24100 calibrator = new KnownGravityNormAccelerometerCalibrator(
24101 invalidGravityNorm, true, biasX, biasY, biasZ,
24102 sx, sy, sz);
24103 fail("IllegalArgumentException expected but not thrown");
24104 } catch (final IllegalArgumentException ignore) {
24105 }
24106 assertNull(calibrator);
24107 }
24108
24109 @Test
24110 public void testConstructor185() throws WrongSizeException {
24111 final Matrix ba = generateBa();
24112 final double biasX = ba.getElementAtIndex(0);
24113 final double biasY = ba.getElementAtIndex(1);
24114 final double biasZ = ba.getElementAtIndex(2);
24115
24116 final Matrix ma = generateMaCommonAxis();
24117 final double sx = ma.getElementAt(0, 0);
24118 final double sy = ma.getElementAt(1, 1);
24119 final double sz = ma.getElementAt(2, 2);
24120
24121 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24122 final double latitude = Math.toRadians(
24123 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24124 final double longitude = Math.toRadians(
24125 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24126 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24127 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24128 final NEDVelocity nedVelocity = new NEDVelocity();
24129 final ECEFPosition ecefPosition = new ECEFPosition();
24130 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24131 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24132 ecefPosition, ecefVelocity);
24133 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24134 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24135 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24136
24137 KnownGravityNormAccelerometerCalibrator calibrator =
24138 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24139 true, biasX, biasY, biasZ, sx, sy, sz,
24140 this);
24141
24142
24143 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24144 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24145 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24146 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24147 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24148 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24149 final Acceleration bx2 = new Acceleration(0.0,
24150 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24151 calibrator.getInitialBiasXAsAcceleration(bx2);
24152 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24153 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24154 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24155 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24156 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24157 final Acceleration by2 = new Acceleration(0.0,
24158 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24159 calibrator.getInitialBiasYAsAcceleration(by2);
24160 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24161 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24162 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24163 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24164 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24165 final Acceleration bz2 = new Acceleration(0.0,
24166 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24167 calibrator.getInitialBiasZAsAcceleration(bz2);
24168 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24169 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24170 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24171 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24172 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24173 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24174 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24175 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24176 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24177 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24178 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24179 final double[] bias1 = calibrator.getInitialBias();
24180 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24181 final double[] bias2 = new double[3];
24182 calibrator.getInitialBias(bias2);
24183 assertArrayEquals(bias1, bias2, 0.0);
24184 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24185 assertEquals(b1, ba);
24186 final Matrix b2 = new Matrix(3, 1);
24187 calibrator.getInitialBiasAsMatrix(b2);
24188 assertEquals(b1, b2);
24189 final Matrix ma1 = calibrator.getInitialMa();
24190 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24191 final Matrix ma2 = new Matrix(3, 3);
24192 calibrator.getInitialMa(ma2);
24193 assertEquals(ma1, ma2);
24194 assertNull(calibrator.getMeasurements());
24195 assertTrue(calibrator.isCommonAxisUsed());
24196 assertSame(calibrator.getListener(), this);
24197 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24198 assertFalse(calibrator.isReady());
24199 assertFalse(calibrator.isRunning());
24200 assertNull(calibrator.getEstimatedBiases());
24201 assertFalse(calibrator.getEstimatedBiases(null));
24202 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24203 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24204 assertNull(calibrator.getEstimatedBiasFx());
24205 assertNull(calibrator.getEstimatedBiasFy());
24206 assertNull(calibrator.getEstimatedBiasFz());
24207 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24208 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24209 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24210 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24211 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24212 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24213 assertNull(calibrator.getEstimatedMa());
24214 assertNull(calibrator.getEstimatedSx());
24215 assertNull(calibrator.getEstimatedSy());
24216 assertNull(calibrator.getEstimatedSz());
24217 assertNull(calibrator.getEstimatedMxy());
24218 assertNull(calibrator.getEstimatedMxz());
24219 assertNull(calibrator.getEstimatedMyx());
24220 assertNull(calibrator.getEstimatedMyz());
24221 assertNull(calibrator.getEstimatedMzx());
24222 assertNull(calibrator.getEstimatedMzy());
24223 assertNull(calibrator.getEstimatedCovariance());
24224 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24225 assertNotNull(calibrator.getGroundTruthGravityNorm());
24226 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24227 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24228 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24229 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24230 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24231 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24232 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24233
24234
24235 final Acceleration invalidGravityNorm = new Acceleration(
24236 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237
24238 calibrator = null;
24239 try {
24240 calibrator = new KnownGravityNormAccelerometerCalibrator(
24241 invalidGravityNorm, true,
24242 biasX, biasY, biasZ,
24243 sx, sy, sz, this);
24244 fail("IllegalArgumentException expected but not thrown");
24245 } catch (final IllegalArgumentException ignore) {
24246 }
24247 assertNull(calibrator);
24248 }
24249
24250 @Test
24251 public void testConstructor186() throws WrongSizeException {
24252 final Collection<StandardDeviationBodyKinematics> measurements =
24253 Collections.emptyList();
24254
24255 final Matrix ba = generateBa();
24256 final double biasX = ba.getElementAtIndex(0);
24257 final double biasY = ba.getElementAtIndex(1);
24258 final double biasZ = ba.getElementAtIndex(2);
24259
24260 final Matrix ma = generateMaCommonAxis();
24261 final double sx = ma.getElementAt(0, 0);
24262 final double sy = ma.getElementAt(1, 1);
24263 final double sz = ma.getElementAt(2, 2);
24264
24265 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24266 final double latitude = Math.toRadians(
24267 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24268 final double longitude = Math.toRadians(
24269 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24270 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24271 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24272 final NEDVelocity nedVelocity = new NEDVelocity();
24273 final ECEFPosition ecefPosition = new ECEFPosition();
24274 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24275 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24276 ecefPosition, ecefVelocity);
24277 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24278 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24279 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24280
24281 KnownGravityNormAccelerometerCalibrator calibrator =
24282 new KnownGravityNormAccelerometerCalibrator(
24283 gravityNorm, measurements,
24284 true, biasX, biasY, biasZ, sx, sy, sz);
24285
24286
24287 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24288 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24289 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24290 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24291 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24292 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24293 final Acceleration bx2 = new Acceleration(0.0,
24294 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24295 calibrator.getInitialBiasXAsAcceleration(bx2);
24296 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24297 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24298 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24299 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24300 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24301 final Acceleration by2 = new Acceleration(0.0,
24302 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24303 calibrator.getInitialBiasYAsAcceleration(by2);
24304 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24305 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24306 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24307 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24308 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24309 final Acceleration bz2 = new Acceleration(0.0,
24310 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24311 calibrator.getInitialBiasZAsAcceleration(bz2);
24312 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24313 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24314 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24315 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24316 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24317 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24318 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24319 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24320 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24321 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24322 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24323 final double[] bias1 = calibrator.getInitialBias();
24324 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24325 final double[] bias2 = new double[3];
24326 calibrator.getInitialBias(bias2);
24327 assertArrayEquals(bias1, bias2, 0.0);
24328 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24329 assertEquals(b1, ba);
24330 final Matrix b2 = new Matrix(3, 1);
24331 calibrator.getInitialBiasAsMatrix(b2);
24332 assertEquals(b1, b2);
24333 final Matrix ma1 = calibrator.getInitialMa();
24334 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24335 final Matrix ma2 = new Matrix(3, 3);
24336 calibrator.getInitialMa(ma2);
24337 assertEquals(ma1, ma2);
24338 assertSame(calibrator.getMeasurements(), measurements);
24339 assertTrue(calibrator.isCommonAxisUsed());
24340 assertNull(calibrator.getListener());
24341 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24342 assertFalse(calibrator.isReady());
24343 assertFalse(calibrator.isRunning());
24344 assertNull(calibrator.getEstimatedBiases());
24345 assertFalse(calibrator.getEstimatedBiases(null));
24346 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24347 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24348 assertNull(calibrator.getEstimatedBiasFx());
24349 assertNull(calibrator.getEstimatedBiasFy());
24350 assertNull(calibrator.getEstimatedBiasFz());
24351 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24352 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24353 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24354 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24355 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24356 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24357 assertNull(calibrator.getEstimatedMa());
24358 assertNull(calibrator.getEstimatedSx());
24359 assertNull(calibrator.getEstimatedSy());
24360 assertNull(calibrator.getEstimatedSz());
24361 assertNull(calibrator.getEstimatedMxy());
24362 assertNull(calibrator.getEstimatedMxz());
24363 assertNull(calibrator.getEstimatedMyx());
24364 assertNull(calibrator.getEstimatedMyz());
24365 assertNull(calibrator.getEstimatedMzx());
24366 assertNull(calibrator.getEstimatedMzy());
24367 assertNull(calibrator.getEstimatedCovariance());
24368 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24369 assertNotNull(calibrator.getGroundTruthGravityNorm());
24370 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24371 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24372 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24373 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24374 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24375 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24376 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24377
24378
24379 final Acceleration invalidGravityNorm = new Acceleration(
24380 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24381
24382 calibrator = null;
24383 try {
24384 calibrator = new KnownGravityNormAccelerometerCalibrator(
24385 invalidGravityNorm, measurements,
24386 true, biasX, biasY, biasZ, sx, sy, sz);
24387 fail("IllegalArgumentException expected but not thrown");
24388 } catch (final IllegalArgumentException ignore) {
24389 }
24390 assertNull(calibrator);
24391 }
24392
24393 @Test
24394 public void testConstructor187() throws WrongSizeException {
24395 final Collection<StandardDeviationBodyKinematics> measurements =
24396 Collections.emptyList();
24397
24398 final Matrix ba = generateBa();
24399 final double biasX = ba.getElementAtIndex(0);
24400 final double biasY = ba.getElementAtIndex(1);
24401 final double biasZ = ba.getElementAtIndex(2);
24402
24403 final Matrix ma = generateMaCommonAxis();
24404 final double sx = ma.getElementAt(0, 0);
24405 final double sy = ma.getElementAt(1, 1);
24406 final double sz = ma.getElementAt(2, 2);
24407
24408 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24409 final double latitude = Math.toRadians(
24410 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24411 final double longitude = Math.toRadians(
24412 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24413 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24414 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24415 final NEDVelocity nedVelocity = new NEDVelocity();
24416 final ECEFPosition ecefPosition = new ECEFPosition();
24417 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24418 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24419 ecefPosition, ecefVelocity);
24420 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24421 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24422 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24423
24424 KnownGravityNormAccelerometerCalibrator calibrator =
24425 new KnownGravityNormAccelerometerCalibrator(
24426 gravityNorm, measurements,
24427 true, biasX, biasY, biasZ, sx, sy, sz,
24428 this);
24429
24430
24431 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24432 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24433 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24434 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24435 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24436 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24437 final Acceleration bx2 = new Acceleration(0.0,
24438 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24439 calibrator.getInitialBiasXAsAcceleration(bx2);
24440 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24441 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24442 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24443 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24444 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24445 final Acceleration by2 = new Acceleration(0.0,
24446 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24447 calibrator.getInitialBiasYAsAcceleration(by2);
24448 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24449 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24450 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24451 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24452 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24453 final Acceleration bz2 = new Acceleration(0.0,
24454 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24455 calibrator.getInitialBiasZAsAcceleration(bz2);
24456 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24457 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24458 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24459 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24460 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24461 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24462 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24463 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24464 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24465 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24466 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24467 final double[] bias1 = calibrator.getInitialBias();
24468 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24469 final double[] bias2 = new double[3];
24470 calibrator.getInitialBias(bias2);
24471 assertArrayEquals(bias1, bias2, 0.0);
24472 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24473 assertEquals(b1, ba);
24474 final Matrix b2 = new Matrix(3, 1);
24475 calibrator.getInitialBiasAsMatrix(b2);
24476 assertEquals(b1, b2);
24477 final Matrix ma1 = calibrator.getInitialMa();
24478 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24479 final Matrix ma2 = new Matrix(3, 3);
24480 calibrator.getInitialMa(ma2);
24481 assertEquals(ma1, ma2);
24482 assertSame(calibrator.getMeasurements(), measurements);
24483 assertTrue(calibrator.isCommonAxisUsed());
24484 assertSame(calibrator.getListener(), this);
24485 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24486 assertFalse(calibrator.isReady());
24487 assertFalse(calibrator.isRunning());
24488 assertNull(calibrator.getEstimatedBiases());
24489 assertFalse(calibrator.getEstimatedBiases(null));
24490 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24491 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24492 assertNull(calibrator.getEstimatedBiasFx());
24493 assertNull(calibrator.getEstimatedBiasFy());
24494 assertNull(calibrator.getEstimatedBiasFz());
24495 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24496 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24497 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24498 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24499 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24500 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24501 assertNull(calibrator.getEstimatedMa());
24502 assertNull(calibrator.getEstimatedSx());
24503 assertNull(calibrator.getEstimatedSy());
24504 assertNull(calibrator.getEstimatedSz());
24505 assertNull(calibrator.getEstimatedMxy());
24506 assertNull(calibrator.getEstimatedMxz());
24507 assertNull(calibrator.getEstimatedMyx());
24508 assertNull(calibrator.getEstimatedMyz());
24509 assertNull(calibrator.getEstimatedMzx());
24510 assertNull(calibrator.getEstimatedMzy());
24511 assertNull(calibrator.getEstimatedCovariance());
24512 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24513 assertNotNull(calibrator.getGroundTruthGravityNorm());
24514 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24515 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24516 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24517 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24518 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24519 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24520 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24521
24522
24523 final Acceleration invalidGravityNorm = new Acceleration(
24524 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24525
24526 calibrator = null;
24527 try {
24528 calibrator = new KnownGravityNormAccelerometerCalibrator(
24529 invalidGravityNorm, measurements,
24530 true, biasX, biasY, biasZ, sx, sy, sz,
24531 this);
24532 fail("IllegalArgumentException expected but not thrown");
24533 } catch (final IllegalArgumentException ignore) {
24534 }
24535 assertNull(calibrator);
24536 }
24537
24538 @Test
24539 public void testConstructor188() throws WrongSizeException {
24540 final Matrix ba = generateBa();
24541 final double biasX = ba.getElementAtIndex(0);
24542 final double biasY = ba.getElementAtIndex(1);
24543 final double biasZ = ba.getElementAtIndex(2);
24544
24545 final Acceleration bx = new Acceleration(biasX,
24546 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24547 final Acceleration by = new Acceleration(biasY,
24548 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24549 final Acceleration bz = new Acceleration(biasZ,
24550 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24551
24552 final Matrix ma = generateMaCommonAxis();
24553 final double sx = ma.getElementAt(0, 0);
24554 final double sy = ma.getElementAt(1, 1);
24555 final double sz = ma.getElementAt(2, 2);
24556
24557 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24558 final double latitude = Math.toRadians(
24559 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24560 final double longitude = Math.toRadians(
24561 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24562 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24563 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24564 final NEDVelocity nedVelocity = new NEDVelocity();
24565 final ECEFPosition ecefPosition = new ECEFPosition();
24566 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24567 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24568 ecefPosition, ecefVelocity);
24569 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24570 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24571 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24572
24573 KnownGravityNormAccelerometerCalibrator calibrator =
24574 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24575 bx, by, bz, sx, sy, sz);
24576
24577
24578 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24579 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24580 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24581 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24582 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24583 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24584 final Acceleration bx2 = new Acceleration(0.0,
24585 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24586 calibrator.getInitialBiasXAsAcceleration(bx2);
24587 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24588 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24589 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24590 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24591 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24592 final Acceleration by2 = new Acceleration(0.0,
24593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24594 calibrator.getInitialBiasYAsAcceleration(by2);
24595 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24596 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24597 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24598 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24599 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24600 final Acceleration bz2 = new Acceleration(0.0,
24601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24602 calibrator.getInitialBiasZAsAcceleration(bz2);
24603 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24604 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24605 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24606 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24607 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24608 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24609 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24610 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24611 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24612 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24613 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24614 final double[] bias1 = calibrator.getInitialBias();
24615 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24616 final double[] bias2 = new double[3];
24617 calibrator.getInitialBias(bias2);
24618 assertArrayEquals(bias1, bias2, 0.0);
24619 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24620 assertEquals(b1, ba);
24621 final Matrix b2 = new Matrix(3, 1);
24622 calibrator.getInitialBiasAsMatrix(b2);
24623 assertEquals(b1, b2);
24624 final Matrix ma1 = calibrator.getInitialMa();
24625 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24626 final Matrix ma2 = new Matrix(3, 3);
24627 calibrator.getInitialMa(ma2);
24628 assertEquals(ma1, ma2);
24629 assertNull(calibrator.getMeasurements());
24630 assertFalse(calibrator.isCommonAxisUsed());
24631 assertNull(calibrator.getListener());
24632 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24633 assertFalse(calibrator.isReady());
24634 assertFalse(calibrator.isRunning());
24635 assertNull(calibrator.getEstimatedBiases());
24636 assertFalse(calibrator.getEstimatedBiases(null));
24637 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24638 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24639 assertNull(calibrator.getEstimatedBiasFx());
24640 assertNull(calibrator.getEstimatedBiasFy());
24641 assertNull(calibrator.getEstimatedBiasFz());
24642 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24643 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24644 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24645 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24646 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24647 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24648 assertNull(calibrator.getEstimatedMa());
24649 assertNull(calibrator.getEstimatedSx());
24650 assertNull(calibrator.getEstimatedSy());
24651 assertNull(calibrator.getEstimatedSz());
24652 assertNull(calibrator.getEstimatedMxy());
24653 assertNull(calibrator.getEstimatedMxz());
24654 assertNull(calibrator.getEstimatedMyx());
24655 assertNull(calibrator.getEstimatedMyz());
24656 assertNull(calibrator.getEstimatedMzx());
24657 assertNull(calibrator.getEstimatedMzy());
24658 assertNull(calibrator.getEstimatedCovariance());
24659 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24660 assertNotNull(calibrator.getGroundTruthGravityNorm());
24661 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24662 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24663 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24664 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24665 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24666 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24667 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24668
24669
24670 final Acceleration invalidGravityNorm = new Acceleration(
24671 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24672
24673 calibrator = null;
24674 try {
24675 calibrator = new KnownGravityNormAccelerometerCalibrator(
24676 invalidGravityNorm,
24677 bx, by, bz, sx, sy, sz);
24678 fail("IllegalArgumentException expected but not thrown");
24679 } catch (final IllegalArgumentException ignore) {
24680 }
24681 assertNull(calibrator);
24682 }
24683
24684 @Test
24685 public void testConstructor189() throws WrongSizeException {
24686 final Matrix ba = generateBa();
24687 final double biasX = ba.getElementAtIndex(0);
24688 final double biasY = ba.getElementAtIndex(1);
24689 final double biasZ = ba.getElementAtIndex(2);
24690
24691 final Acceleration bx = new Acceleration(biasX,
24692 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693 final Acceleration by = new Acceleration(biasY,
24694 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24695 final Acceleration bz = new Acceleration(biasZ,
24696 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24697
24698 final Matrix ma = generateMaCommonAxis();
24699 final double sx = ma.getElementAt(0, 0);
24700 final double sy = ma.getElementAt(1, 1);
24701 final double sz = ma.getElementAt(2, 2);
24702
24703 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24704 final double latitude = Math.toRadians(
24705 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24706 final double longitude = Math.toRadians(
24707 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24708 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24709 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24710 final NEDVelocity nedVelocity = new NEDVelocity();
24711 final ECEFPosition ecefPosition = new ECEFPosition();
24712 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24713 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24714 ecefPosition, ecefVelocity);
24715 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24716 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24717 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24718
24719 KnownGravityNormAccelerometerCalibrator calibrator =
24720 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24721 bx, by, bz, sx, sy, sz, this);
24722
24723
24724 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24725 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24726 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24727 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24728 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24729 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24730 final Acceleration bx2 = new Acceleration(0.0,
24731 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24732 calibrator.getInitialBiasXAsAcceleration(bx2);
24733 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24734 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24735 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24736 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24737 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24738 final Acceleration by2 = new Acceleration(0.0,
24739 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24740 calibrator.getInitialBiasYAsAcceleration(by2);
24741 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24742 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24743 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24744 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24745 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24746 final Acceleration bz2 = new Acceleration(0.0,
24747 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24748 calibrator.getInitialBiasZAsAcceleration(bz2);
24749 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24750 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24751 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24752 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24753 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24754 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24755 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24756 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24757 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24758 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24759 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24760 final double[] bias1 = calibrator.getInitialBias();
24761 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24762 final double[] bias2 = new double[3];
24763 calibrator.getInitialBias(bias2);
24764 assertArrayEquals(bias1, bias2, 0.0);
24765 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24766 assertEquals(b1, ba);
24767 final Matrix b2 = new Matrix(3, 1);
24768 calibrator.getInitialBiasAsMatrix(b2);
24769 assertEquals(b1, b2);
24770 final Matrix ma1 = calibrator.getInitialMa();
24771 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24772 final Matrix ma2 = new Matrix(3, 3);
24773 calibrator.getInitialMa(ma2);
24774 assertEquals(ma1, ma2);
24775 assertNull(calibrator.getMeasurements());
24776 assertFalse(calibrator.isCommonAxisUsed());
24777 assertSame(calibrator.getListener(), this);
24778 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24779 assertFalse(calibrator.isReady());
24780 assertFalse(calibrator.isRunning());
24781 assertNull(calibrator.getEstimatedBiases());
24782 assertFalse(calibrator.getEstimatedBiases(null));
24783 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24784 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24785 assertNull(calibrator.getEstimatedBiasFx());
24786 assertNull(calibrator.getEstimatedBiasFy());
24787 assertNull(calibrator.getEstimatedBiasFz());
24788 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24789 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24790 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24791 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24792 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24793 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24794 assertNull(calibrator.getEstimatedMa());
24795 assertNull(calibrator.getEstimatedSx());
24796 assertNull(calibrator.getEstimatedSy());
24797 assertNull(calibrator.getEstimatedSz());
24798 assertNull(calibrator.getEstimatedMxy());
24799 assertNull(calibrator.getEstimatedMxz());
24800 assertNull(calibrator.getEstimatedMyx());
24801 assertNull(calibrator.getEstimatedMyz());
24802 assertNull(calibrator.getEstimatedMzx());
24803 assertNull(calibrator.getEstimatedMzy());
24804 assertNull(calibrator.getEstimatedCovariance());
24805 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24806 assertNotNull(calibrator.getGroundTruthGravityNorm());
24807 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24808 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24809 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24810 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24811 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24812 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24813 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24814
24815
24816 final Acceleration invalidGravityNorm = new Acceleration(
24817 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24818
24819 calibrator = null;
24820 try {
24821 calibrator = new KnownGravityNormAccelerometerCalibrator(
24822 invalidGravityNorm, bx, by, bz, sx, sy, sz,
24823 this);
24824 fail("IllegalArgumentException expected but not thrown");
24825 } catch (final IllegalArgumentException ignore) {
24826 }
24827 assertNull(calibrator);
24828 }
24829
24830 @Test
24831 public void testConstructor190() throws WrongSizeException {
24832 final Collection<StandardDeviationBodyKinematics> measurements =
24833 Collections.emptyList();
24834
24835 final Matrix ba = generateBa();
24836 final double biasX = ba.getElementAtIndex(0);
24837 final double biasY = ba.getElementAtIndex(1);
24838 final double biasZ = ba.getElementAtIndex(2);
24839
24840 final Acceleration bx = new Acceleration(biasX,
24841 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24842 final Acceleration by = new Acceleration(biasY,
24843 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24844 final Acceleration bz = new Acceleration(biasZ,
24845 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24846
24847 final Matrix ma = generateMaCommonAxis();
24848 final double sx = ma.getElementAt(0, 0);
24849 final double sy = ma.getElementAt(1, 1);
24850 final double sz = ma.getElementAt(2, 2);
24851
24852 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24853 final double latitude = Math.toRadians(
24854 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24855 final double longitude = Math.toRadians(
24856 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24857 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24858 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24859 final NEDVelocity nedVelocity = new NEDVelocity();
24860 final ECEFPosition ecefPosition = new ECEFPosition();
24861 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24862 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24863 ecefPosition, ecefVelocity);
24864 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24865 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24866 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24867
24868 KnownGravityNormAccelerometerCalibrator calibrator =
24869 new KnownGravityNormAccelerometerCalibrator(
24870 gravityNorm, measurements,
24871 bx, by, bz, sx, sy, sz);
24872
24873
24874 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24875 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24876 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24877 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24878 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24879 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24880 final Acceleration bx2 = new Acceleration(0.0,
24881 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24882 calibrator.getInitialBiasXAsAcceleration(bx2);
24883 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24884 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24885 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24886 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24887 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24888 final Acceleration by2 = new Acceleration(0.0,
24889 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24890 calibrator.getInitialBiasYAsAcceleration(by2);
24891 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24892 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24893 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24894 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24895 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24896 final Acceleration bz2 = new Acceleration(0.0,
24897 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24898 calibrator.getInitialBiasZAsAcceleration(bz2);
24899 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24900 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24901 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24902 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24903 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24904 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24905 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24906 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24907 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24908 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24909 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24910 final double[] bias1 = calibrator.getInitialBias();
24911 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24912 final double[] bias2 = new double[3];
24913 calibrator.getInitialBias(bias2);
24914 assertArrayEquals(bias1, bias2, 0.0);
24915 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24916 assertEquals(b1, ba);
24917 final Matrix b2 = new Matrix(3, 1);
24918 calibrator.getInitialBiasAsMatrix(b2);
24919 assertEquals(b1, b2);
24920 final Matrix ma1 = calibrator.getInitialMa();
24921 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24922 final Matrix ma2 = new Matrix(3, 3);
24923 calibrator.getInitialMa(ma2);
24924 assertEquals(ma1, ma2);
24925 assertSame(calibrator.getMeasurements(), measurements);
24926 assertFalse(calibrator.isCommonAxisUsed());
24927 assertNull(calibrator.getListener());
24928 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24929 assertFalse(calibrator.isReady());
24930 assertFalse(calibrator.isRunning());
24931 assertNull(calibrator.getEstimatedBiases());
24932 assertFalse(calibrator.getEstimatedBiases(null));
24933 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24934 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24935 assertNull(calibrator.getEstimatedBiasFx());
24936 assertNull(calibrator.getEstimatedBiasFy());
24937 assertNull(calibrator.getEstimatedBiasFz());
24938 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24939 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24940 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24941 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24942 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24943 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24944 assertNull(calibrator.getEstimatedMa());
24945 assertNull(calibrator.getEstimatedSx());
24946 assertNull(calibrator.getEstimatedSy());
24947 assertNull(calibrator.getEstimatedSz());
24948 assertNull(calibrator.getEstimatedMxy());
24949 assertNull(calibrator.getEstimatedMxz());
24950 assertNull(calibrator.getEstimatedMyx());
24951 assertNull(calibrator.getEstimatedMyz());
24952 assertNull(calibrator.getEstimatedMzx());
24953 assertNull(calibrator.getEstimatedMzy());
24954 assertNull(calibrator.getEstimatedCovariance());
24955 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24956 assertNotNull(calibrator.getGroundTruthGravityNorm());
24957 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24958 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24959 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24960 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24961 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24962 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24963 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24964
24965
24966 final Acceleration invalidGravityNorm = new Acceleration(
24967 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24968
24969 calibrator = null;
24970 try {
24971 calibrator = new KnownGravityNormAccelerometerCalibrator(
24972 invalidGravityNorm, measurements,
24973 bx, by, bz, sx, sy, sz);
24974 fail("IllegalArgumentException expected but not thrown");
24975 } catch (final IllegalArgumentException ignore) {
24976 }
24977 assertNull(calibrator);
24978 }
24979
24980 @Test
24981 public void testConstructor191() throws WrongSizeException {
24982 final Collection<StandardDeviationBodyKinematics> measurements =
24983 Collections.emptyList();
24984
24985 final Matrix ba = generateBa();
24986 final double biasX = ba.getElementAtIndex(0);
24987 final double biasY = ba.getElementAtIndex(1);
24988 final double biasZ = ba.getElementAtIndex(2);
24989
24990 final Acceleration bx = new Acceleration(biasX,
24991 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24992 final Acceleration by = new Acceleration(biasY,
24993 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24994 final Acceleration bz = new Acceleration(biasZ,
24995 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24996
24997 final Matrix ma = generateMaCommonAxis();
24998 final double sx = ma.getElementAt(0, 0);
24999 final double sy = ma.getElementAt(1, 1);
25000 final double sz = ma.getElementAt(2, 2);
25001
25002 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25003 final double latitude = Math.toRadians(
25004 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25005 final double longitude = Math.toRadians(
25006 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25007 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25008 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25009 final NEDVelocity nedVelocity = new NEDVelocity();
25010 final ECEFPosition ecefPosition = new ECEFPosition();
25011 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25012 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25013 ecefPosition, ecefVelocity);
25014 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25015 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25016 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25017
25018 KnownGravityNormAccelerometerCalibrator calibrator =
25019 new KnownGravityNormAccelerometerCalibrator(
25020 gravityNorm, measurements,
25021 bx, by, bz, sx, sy, sz, this);
25022
25023
25024 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25025 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25026 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25027 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25028 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25029 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25030 final Acceleration bx2 = new Acceleration(0.0,
25031 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25032 calibrator.getInitialBiasXAsAcceleration(bx2);
25033 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25034 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25035 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25036 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25037 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25038 final Acceleration by2 = new Acceleration(0.0,
25039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25040 calibrator.getInitialBiasYAsAcceleration(by2);
25041 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25042 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25043 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25044 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25045 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25046 final Acceleration bz2 = new Acceleration(0.0,
25047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25048 calibrator.getInitialBiasZAsAcceleration(bz2);
25049 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25050 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25051 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25052 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25053 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25054 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25055 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25056 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25057 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25058 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25059 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25060 final double[] bias1 = calibrator.getInitialBias();
25061 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25062 final double[] bias2 = new double[3];
25063 calibrator.getInitialBias(bias2);
25064 assertArrayEquals(bias1, bias2, 0.0);
25065 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25066 assertEquals(b1, ba);
25067 final Matrix b2 = new Matrix(3, 1);
25068 calibrator.getInitialBiasAsMatrix(b2);
25069 assertEquals(b1, b2);
25070 final Matrix ma1 = calibrator.getInitialMa();
25071 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25072 final Matrix ma2 = new Matrix(3, 3);
25073 calibrator.getInitialMa(ma2);
25074 assertEquals(ma1, ma2);
25075 assertSame(calibrator.getMeasurements(), measurements);
25076 assertFalse(calibrator.isCommonAxisUsed());
25077 assertSame(calibrator.getListener(), this);
25078 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25079 assertFalse(calibrator.isReady());
25080 assertFalse(calibrator.isRunning());
25081 assertNull(calibrator.getEstimatedBiases());
25082 assertFalse(calibrator.getEstimatedBiases(null));
25083 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25084 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25085 assertNull(calibrator.getEstimatedBiasFx());
25086 assertNull(calibrator.getEstimatedBiasFy());
25087 assertNull(calibrator.getEstimatedBiasFz());
25088 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25089 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25090 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25091 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25092 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25093 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25094 assertNull(calibrator.getEstimatedMa());
25095 assertNull(calibrator.getEstimatedSx());
25096 assertNull(calibrator.getEstimatedSy());
25097 assertNull(calibrator.getEstimatedSz());
25098 assertNull(calibrator.getEstimatedMxy());
25099 assertNull(calibrator.getEstimatedMxz());
25100 assertNull(calibrator.getEstimatedMyx());
25101 assertNull(calibrator.getEstimatedMyz());
25102 assertNull(calibrator.getEstimatedMzx());
25103 assertNull(calibrator.getEstimatedMzy());
25104 assertNull(calibrator.getEstimatedCovariance());
25105 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25106 assertNotNull(calibrator.getGroundTruthGravityNorm());
25107 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25108 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25109 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25110 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25111 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25112 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25113 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25114
25115
25116 final Acceleration invalidGravityNorm = new Acceleration(
25117 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25118
25119 calibrator = null;
25120 try {
25121 calibrator = new KnownGravityNormAccelerometerCalibrator(
25122 invalidGravityNorm, measurements,
25123 bx, by, bz, sx, sy, sz, this);
25124 fail("IllegalArgumentException expected but not thrown");
25125 } catch (final IllegalArgumentException ignore) {
25126 }
25127 assertNull(calibrator);
25128 }
25129
25130 @Test
25131 public void testConstructor192() throws WrongSizeException {
25132 final Matrix ba = generateBa();
25133 final double biasX = ba.getElementAtIndex(0);
25134 final double biasY = ba.getElementAtIndex(1);
25135 final double biasZ = ba.getElementAtIndex(2);
25136
25137 final Acceleration bx = new Acceleration(biasX,
25138 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25139 final Acceleration by = new Acceleration(biasY,
25140 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25141 final Acceleration bz = new Acceleration(biasZ,
25142 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25143
25144 final Matrix ma = generateMaCommonAxis();
25145 final double sx = ma.getElementAt(0, 0);
25146 final double sy = ma.getElementAt(1, 1);
25147 final double sz = ma.getElementAt(2, 2);
25148
25149 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25150 final double latitude = Math.toRadians(
25151 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25152 final double longitude = Math.toRadians(
25153 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25154 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25155 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25156 final NEDVelocity nedVelocity = new NEDVelocity();
25157 final ECEFPosition ecefPosition = new ECEFPosition();
25158 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25159 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25160 ecefPosition, ecefVelocity);
25161 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25162 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25163 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25164
25165 KnownGravityNormAccelerometerCalibrator calibrator =
25166 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25167 true, bx, by, bz, sx, sy, sz);
25168
25169
25170 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25171 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25172 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25173 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25174 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25175 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25176 final Acceleration bx2 = new Acceleration(0.0,
25177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25178 calibrator.getInitialBiasXAsAcceleration(bx2);
25179 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25180 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25181 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25182 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25183 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25184 final Acceleration by2 = new Acceleration(0.0,
25185 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25186 calibrator.getInitialBiasYAsAcceleration(by2);
25187 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25188 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25189 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25190 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25191 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25192 final Acceleration bz2 = new Acceleration(0.0,
25193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25194 calibrator.getInitialBiasZAsAcceleration(bz2);
25195 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25196 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25197 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25198 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25199 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25200 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25201 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25202 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25203 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25204 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25205 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25206 final double[] bias1 = calibrator.getInitialBias();
25207 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25208 final double[] bias2 = new double[3];
25209 calibrator.getInitialBias(bias2);
25210 assertArrayEquals(bias1, bias2, 0.0);
25211 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25212 assertEquals(b1, ba);
25213 final Matrix b2 = new Matrix(3, 1);
25214 calibrator.getInitialBiasAsMatrix(b2);
25215 assertEquals(b1, b2);
25216 final Matrix ma1 = calibrator.getInitialMa();
25217 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25218 final Matrix ma2 = new Matrix(3, 3);
25219 calibrator.getInitialMa(ma2);
25220 assertEquals(ma1, ma2);
25221 assertNull(calibrator.getMeasurements());
25222 assertTrue(calibrator.isCommonAxisUsed());
25223 assertNull(calibrator.getListener());
25224 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25225 assertFalse(calibrator.isReady());
25226 assertFalse(calibrator.isRunning());
25227 assertNull(calibrator.getEstimatedBiases());
25228 assertFalse(calibrator.getEstimatedBiases(null));
25229 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25230 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25231 assertNull(calibrator.getEstimatedBiasFx());
25232 assertNull(calibrator.getEstimatedBiasFy());
25233 assertNull(calibrator.getEstimatedBiasFz());
25234 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25235 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25236 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25237 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25238 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25239 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25240 assertNull(calibrator.getEstimatedMa());
25241 assertNull(calibrator.getEstimatedSx());
25242 assertNull(calibrator.getEstimatedSy());
25243 assertNull(calibrator.getEstimatedSz());
25244 assertNull(calibrator.getEstimatedMxy());
25245 assertNull(calibrator.getEstimatedMxz());
25246 assertNull(calibrator.getEstimatedMyx());
25247 assertNull(calibrator.getEstimatedMyz());
25248 assertNull(calibrator.getEstimatedMzx());
25249 assertNull(calibrator.getEstimatedMzy());
25250 assertNull(calibrator.getEstimatedCovariance());
25251 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25252 assertNotNull(calibrator.getGroundTruthGravityNorm());
25253 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25254 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25255 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25256 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25257 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25258 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25259 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25260
25261
25262 final Acceleration invalidGravityNorm = new Acceleration(
25263 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25264
25265 calibrator = null;
25266 try {
25267 calibrator = new KnownGravityNormAccelerometerCalibrator(
25268 invalidGravityNorm, true,
25269 bx, by, bz, sx, sy, sz);
25270 fail("IllegalArgumentException expected but not thrown");
25271 } catch (final IllegalArgumentException ignore) {
25272 }
25273 assertNull(calibrator);
25274 }
25275
25276 @Test
25277 public void testConstructor193() throws WrongSizeException {
25278 final Matrix ba = generateBa();
25279 final double biasX = ba.getElementAtIndex(0);
25280 final double biasY = ba.getElementAtIndex(1);
25281 final double biasZ = ba.getElementAtIndex(2);
25282
25283 final Acceleration bx = new Acceleration(biasX,
25284 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25285 final Acceleration by = new Acceleration(biasY,
25286 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25287 final Acceleration bz = new Acceleration(biasZ,
25288 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25289
25290 final Matrix ma = generateMaCommonAxis();
25291 final double sx = ma.getElementAt(0, 0);
25292 final double sy = ma.getElementAt(1, 1);
25293 final double sz = ma.getElementAt(2, 2);
25294
25295 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25296 final double latitude = Math.toRadians(
25297 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25298 final double longitude = Math.toRadians(
25299 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25300 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25301 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25302 final NEDVelocity nedVelocity = new NEDVelocity();
25303 final ECEFPosition ecefPosition = new ECEFPosition();
25304 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25305 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25306 ecefPosition, ecefVelocity);
25307 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25308 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25309 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25310
25311 KnownGravityNormAccelerometerCalibrator calibrator =
25312 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25313 true, bx, by, bz, sx, sy, sz, this);
25314
25315
25316 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25317 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25318 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25319 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25320 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25321 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25322 final Acceleration bx2 = new Acceleration(0.0,
25323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25324 calibrator.getInitialBiasXAsAcceleration(bx2);
25325 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25326 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25327 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25328 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25329 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25330 final Acceleration by2 = new Acceleration(0.0,
25331 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25332 calibrator.getInitialBiasYAsAcceleration(by2);
25333 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25334 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25335 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25336 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25337 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25338 final Acceleration bz2 = new Acceleration(0.0,
25339 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25340 calibrator.getInitialBiasZAsAcceleration(bz2);
25341 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25342 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25343 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25344 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25345 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25346 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25347 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25348 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25349 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25350 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25351 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25352 final double[] bias1 = calibrator.getInitialBias();
25353 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25354 final double[] bias2 = new double[3];
25355 calibrator.getInitialBias(bias2);
25356 assertArrayEquals(bias1, bias2, 0.0);
25357 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25358 assertEquals(b1, ba);
25359 final Matrix b2 = new Matrix(3, 1);
25360 calibrator.getInitialBiasAsMatrix(b2);
25361 assertEquals(b1, b2);
25362 final Matrix ma1 = calibrator.getInitialMa();
25363 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25364 final Matrix ma2 = new Matrix(3, 3);
25365 calibrator.getInitialMa(ma2);
25366 assertEquals(ma1, ma2);
25367 assertNull(calibrator.getMeasurements());
25368 assertTrue(calibrator.isCommonAxisUsed());
25369 assertSame(calibrator.getListener(), this);
25370 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25371 assertFalse(calibrator.isReady());
25372 assertFalse(calibrator.isRunning());
25373 assertNull(calibrator.getEstimatedBiases());
25374 assertFalse(calibrator.getEstimatedBiases(null));
25375 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25376 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25377 assertNull(calibrator.getEstimatedBiasFx());
25378 assertNull(calibrator.getEstimatedBiasFy());
25379 assertNull(calibrator.getEstimatedBiasFz());
25380 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25381 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25382 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25383 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25384 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25385 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25386 assertNull(calibrator.getEstimatedMa());
25387 assertNull(calibrator.getEstimatedSx());
25388 assertNull(calibrator.getEstimatedSy());
25389 assertNull(calibrator.getEstimatedSz());
25390 assertNull(calibrator.getEstimatedMxy());
25391 assertNull(calibrator.getEstimatedMxz());
25392 assertNull(calibrator.getEstimatedMyx());
25393 assertNull(calibrator.getEstimatedMyz());
25394 assertNull(calibrator.getEstimatedMzx());
25395 assertNull(calibrator.getEstimatedMzy());
25396 assertNull(calibrator.getEstimatedCovariance());
25397 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25398 assertNotNull(calibrator.getGroundTruthGravityNorm());
25399 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25400 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25401 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25402 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25403 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25404 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25405 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25406
25407
25408 final Acceleration invalidGravityNorm = new Acceleration(
25409 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25410
25411 calibrator = null;
25412 try {
25413 calibrator = new KnownGravityNormAccelerometerCalibrator(
25414 invalidGravityNorm, true,
25415 bx, by, bz, sx, sy, sz, this);
25416 fail("IllegalArgumentException expected but not thrown");
25417 } catch (final IllegalArgumentException ignore) {
25418 }
25419 assertNull(calibrator);
25420 }
25421
25422 @Test
25423 public void testConstructor194() throws WrongSizeException {
25424 final Collection<StandardDeviationBodyKinematics> measurements =
25425 Collections.emptyList();
25426
25427 final Matrix ba = generateBa();
25428 final double biasX = ba.getElementAtIndex(0);
25429 final double biasY = ba.getElementAtIndex(1);
25430 final double biasZ = ba.getElementAtIndex(2);
25431
25432 final Acceleration bx = new Acceleration(biasX,
25433 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25434 final Acceleration by = new Acceleration(biasY,
25435 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25436 final Acceleration bz = new Acceleration(biasZ,
25437 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25438
25439 final Matrix ma = generateMaCommonAxis();
25440 final double sx = ma.getElementAt(0, 0);
25441 final double sy = ma.getElementAt(1, 1);
25442 final double sz = ma.getElementAt(2, 2);
25443
25444 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25445 final double latitude = Math.toRadians(
25446 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25447 final double longitude = Math.toRadians(
25448 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25449 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25450 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25451 final NEDVelocity nedVelocity = new NEDVelocity();
25452 final ECEFPosition ecefPosition = new ECEFPosition();
25453 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25454 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25455 ecefPosition, ecefVelocity);
25456 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25457 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25458 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25459
25460 KnownGravityNormAccelerometerCalibrator calibrator =
25461 new KnownGravityNormAccelerometerCalibrator(
25462 gravityNorm, measurements,
25463 true, bx, by, bz, sx, sy, sz);
25464
25465
25466 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25467 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25468 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25469 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25470 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25471 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25472 final Acceleration bx2 = new Acceleration(0.0,
25473 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25474 calibrator.getInitialBiasXAsAcceleration(bx2);
25475 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25476 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25477 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25478 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25479 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25480 final Acceleration by2 = new Acceleration(0.0,
25481 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25482 calibrator.getInitialBiasYAsAcceleration(by2);
25483 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25484 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25485 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25486 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25487 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25488 final Acceleration bz2 = new Acceleration(0.0,
25489 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25490 calibrator.getInitialBiasZAsAcceleration(bz2);
25491 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25492 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25493 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25494 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25495 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25496 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25497 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25498 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25499 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25500 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25501 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25502 final double[] bias1 = calibrator.getInitialBias();
25503 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25504 final double[] bias2 = new double[3];
25505 calibrator.getInitialBias(bias2);
25506 assertArrayEquals(bias1, bias2, 0.0);
25507 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25508 assertEquals(b1, ba);
25509 final Matrix b2 = new Matrix(3, 1);
25510 calibrator.getInitialBiasAsMatrix(b2);
25511 assertEquals(b1, b2);
25512 final Matrix ma1 = calibrator.getInitialMa();
25513 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25514 final Matrix ma2 = new Matrix(3, 3);
25515 calibrator.getInitialMa(ma2);
25516 assertEquals(ma1, ma2);
25517 assertSame(calibrator.getMeasurements(), measurements);
25518 assertTrue(calibrator.isCommonAxisUsed());
25519 assertNull(calibrator.getListener());
25520 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25521 assertFalse(calibrator.isReady());
25522 assertFalse(calibrator.isRunning());
25523 assertNull(calibrator.getEstimatedBiases());
25524 assertFalse(calibrator.getEstimatedBiases(null));
25525 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25526 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25527 assertNull(calibrator.getEstimatedBiasFx());
25528 assertNull(calibrator.getEstimatedBiasFy());
25529 assertNull(calibrator.getEstimatedBiasFz());
25530 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25531 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25532 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25533 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25534 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25535 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25536 assertNull(calibrator.getEstimatedMa());
25537 assertNull(calibrator.getEstimatedSx());
25538 assertNull(calibrator.getEstimatedSy());
25539 assertNull(calibrator.getEstimatedSz());
25540 assertNull(calibrator.getEstimatedMxy());
25541 assertNull(calibrator.getEstimatedMxz());
25542 assertNull(calibrator.getEstimatedMyx());
25543 assertNull(calibrator.getEstimatedMyz());
25544 assertNull(calibrator.getEstimatedMzx());
25545 assertNull(calibrator.getEstimatedMzy());
25546 assertNull(calibrator.getEstimatedCovariance());
25547 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25548 assertNotNull(calibrator.getGroundTruthGravityNorm());
25549 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25550 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25551 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25552 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25553 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25554 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25555 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25556
25557
25558 final Acceleration invalidGravityNorm = new Acceleration(
25559 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25560
25561 calibrator = null;
25562 try {
25563 calibrator = new KnownGravityNormAccelerometerCalibrator(
25564 invalidGravityNorm, measurements,
25565 true, bx, by, bz, sx, sy, sz);
25566 fail("IllegalArgumentException expected but not thrown");
25567 } catch (final IllegalArgumentException ignore) {
25568 }
25569 assertNull(calibrator);
25570 }
25571
25572 @Test
25573 public void testConstructor195() throws WrongSizeException {
25574 final Collection<StandardDeviationBodyKinematics> measurements =
25575 Collections.emptyList();
25576
25577 final Matrix ba = generateBa();
25578 final double biasX = ba.getElementAtIndex(0);
25579 final double biasY = ba.getElementAtIndex(1);
25580 final double biasZ = ba.getElementAtIndex(2);
25581
25582 final Acceleration bx = new Acceleration(biasX,
25583 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25584 final Acceleration by = new Acceleration(biasY,
25585 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25586 final Acceleration bz = new Acceleration(biasZ,
25587 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25588
25589 final Matrix ma = generateMaCommonAxis();
25590 final double sx = ma.getElementAt(0, 0);
25591 final double sy = ma.getElementAt(1, 1);
25592 final double sz = ma.getElementAt(2, 2);
25593
25594 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25595 final double latitude = Math.toRadians(
25596 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25597 final double longitude = Math.toRadians(
25598 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25599 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25600 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25601 final NEDVelocity nedVelocity = new NEDVelocity();
25602 final ECEFPosition ecefPosition = new ECEFPosition();
25603 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25604 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25605 ecefPosition, ecefVelocity);
25606 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25607 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25608 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25609
25610 KnownGravityNormAccelerometerCalibrator calibrator =
25611 new KnownGravityNormAccelerometerCalibrator(
25612 gravityNorm, measurements,
25613 true, bx, by, bz, sx, sy, sz, this);
25614
25615
25616 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25617 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25618 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25619 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25620 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25621 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25622 final Acceleration bx2 = new Acceleration(0.0,
25623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25624 calibrator.getInitialBiasXAsAcceleration(bx2);
25625 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25626 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25627 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25628 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25629 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25630 final Acceleration by2 = new Acceleration(0.0,
25631 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25632 calibrator.getInitialBiasYAsAcceleration(by2);
25633 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25634 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25635 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25636 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25637 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25638 final Acceleration bz2 = new Acceleration(0.0,
25639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25640 calibrator.getInitialBiasZAsAcceleration(bz2);
25641 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25642 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25643 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25644 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25645 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25646 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25647 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25648 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25649 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25650 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25651 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25652 final double[] bias1 = calibrator.getInitialBias();
25653 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25654 final double[] bias2 = new double[3];
25655 calibrator.getInitialBias(bias2);
25656 assertArrayEquals(bias1, bias2, 0.0);
25657 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25658 assertEquals(b1, ba);
25659 final Matrix b2 = new Matrix(3, 1);
25660 calibrator.getInitialBiasAsMatrix(b2);
25661 assertEquals(b1, b2);
25662 final Matrix ma1 = calibrator.getInitialMa();
25663 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25664 final Matrix ma2 = new Matrix(3, 3);
25665 calibrator.getInitialMa(ma2);
25666 assertEquals(ma1, ma2);
25667 assertSame(calibrator.getMeasurements(), measurements);
25668 assertTrue(calibrator.isCommonAxisUsed());
25669 assertSame(calibrator.getListener(), this);
25670 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25671 assertFalse(calibrator.isReady());
25672 assertFalse(calibrator.isRunning());
25673 assertNull(calibrator.getEstimatedBiases());
25674 assertFalse(calibrator.getEstimatedBiases(null));
25675 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25676 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25677 assertNull(calibrator.getEstimatedBiasFx());
25678 assertNull(calibrator.getEstimatedBiasFy());
25679 assertNull(calibrator.getEstimatedBiasFz());
25680 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25681 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25682 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25683 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25684 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25685 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25686 assertNull(calibrator.getEstimatedMa());
25687 assertNull(calibrator.getEstimatedSx());
25688 assertNull(calibrator.getEstimatedSy());
25689 assertNull(calibrator.getEstimatedSz());
25690 assertNull(calibrator.getEstimatedMxy());
25691 assertNull(calibrator.getEstimatedMxz());
25692 assertNull(calibrator.getEstimatedMyx());
25693 assertNull(calibrator.getEstimatedMyz());
25694 assertNull(calibrator.getEstimatedMzx());
25695 assertNull(calibrator.getEstimatedMzy());
25696 assertNull(calibrator.getEstimatedCovariance());
25697 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25698 assertNotNull(calibrator.getGroundTruthGravityNorm());
25699 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25700 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25701 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25702 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25703 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25704 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25705 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25706
25707
25708 final Acceleration invalidGravityNorm = new Acceleration(
25709 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25710
25711 calibrator = null;
25712 try {
25713 calibrator = new KnownGravityNormAccelerometerCalibrator(
25714 invalidGravityNorm, measurements,
25715 true, bx, by, bz, sx, sy, sz, this);
25716 fail("IllegalArgumentException expected but not thrown");
25717 } catch (final IllegalArgumentException ignore) {
25718 }
25719 assertNull(calibrator);
25720 }
25721
25722 @Test
25723 public void testConstructor196() throws WrongSizeException {
25724 final Matrix ba = generateBa();
25725 final double biasX = ba.getElementAtIndex(0);
25726 final double biasY = ba.getElementAtIndex(1);
25727 final double biasZ = ba.getElementAtIndex(2);
25728
25729 final Matrix ma = generateMaCommonAxis();
25730 final double sx = ma.getElementAt(0, 0);
25731 final double sy = ma.getElementAt(1, 1);
25732 final double sz = ma.getElementAt(2, 2);
25733 final double mxy = ma.getElementAt(0, 1);
25734 final double mxz = ma.getElementAt(0, 2);
25735 final double myx = ma.getElementAt(1, 0);
25736 final double myz = ma.getElementAt(1, 2);
25737 final double mzx = ma.getElementAt(2, 0);
25738 final double mzy = ma.getElementAt(2, 1);
25739
25740 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25741 final double latitude = Math.toRadians(
25742 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25743 final double longitude = Math.toRadians(
25744 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25745 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25746 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25747 final NEDVelocity nedVelocity = new NEDVelocity();
25748 final ECEFPosition ecefPosition = new ECEFPosition();
25749 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25750 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25751 ecefPosition, ecefVelocity);
25752 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25753 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25754 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25755
25756 KnownGravityNormAccelerometerCalibrator calibrator =
25757 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25758 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25759 myx, myz, mzx, mzy);
25760
25761
25762 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25763 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25764 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25765 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25766 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25767 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25768 final Acceleration bx2 = new Acceleration(0.0,
25769 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25770 calibrator.getInitialBiasXAsAcceleration(bx2);
25771 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25772 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25773 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25774 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25775 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25776 final Acceleration by2 = new Acceleration(0.0,
25777 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25778 calibrator.getInitialBiasYAsAcceleration(by2);
25779 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25780 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25781 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25782 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25783 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25784 final Acceleration bz2 = new Acceleration(0.0,
25785 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25786 calibrator.getInitialBiasZAsAcceleration(bz2);
25787 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25788 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25789 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25790 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25791 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25792 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25793 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25794 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25795 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25796 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25797 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25798 final double[] bias1 = calibrator.getInitialBias();
25799 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25800 final double[] bias2 = new double[3];
25801 calibrator.getInitialBias(bias2);
25802 assertArrayEquals(bias1, bias2, 0.0);
25803 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25804 assertEquals(b1, ba);
25805 final Matrix b2 = new Matrix(3, 1);
25806 calibrator.getInitialBiasAsMatrix(b2);
25807 assertEquals(b1, b2);
25808 final Matrix ma1 = new Matrix(3, 3);
25809 ma1.setSubmatrix(0, 0,
25810 2, 2,
25811 new double[]{sx, myx, mzx,
25812 mxy, sy, mzy,
25813 mxz, myz, sz});
25814 assertEquals(calibrator.getInitialMa(), ma1);
25815 final Matrix ma2 = new Matrix(3, 3);
25816 calibrator.getInitialMa(ma2);
25817 assertEquals(ma1, ma2);
25818 assertNull(calibrator.getMeasurements());
25819 assertFalse(calibrator.isCommonAxisUsed());
25820 assertNull(calibrator.getListener());
25821 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25822 assertFalse(calibrator.isReady());
25823 assertFalse(calibrator.isRunning());
25824 assertNull(calibrator.getEstimatedBiases());
25825 assertFalse(calibrator.getEstimatedBiases(null));
25826 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25827 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25828 assertNull(calibrator.getEstimatedBiasFx());
25829 assertNull(calibrator.getEstimatedBiasFy());
25830 assertNull(calibrator.getEstimatedBiasFz());
25831 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25832 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25833 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25834 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25835 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25836 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25837 assertNull(calibrator.getEstimatedMa());
25838 assertNull(calibrator.getEstimatedSx());
25839 assertNull(calibrator.getEstimatedSy());
25840 assertNull(calibrator.getEstimatedSz());
25841 assertNull(calibrator.getEstimatedMxy());
25842 assertNull(calibrator.getEstimatedMxz());
25843 assertNull(calibrator.getEstimatedMyx());
25844 assertNull(calibrator.getEstimatedMyz());
25845 assertNull(calibrator.getEstimatedMzx());
25846 assertNull(calibrator.getEstimatedMzy());
25847 assertNull(calibrator.getEstimatedCovariance());
25848 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25849 assertNotNull(calibrator.getGroundTruthGravityNorm());
25850 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25851 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25852 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25853 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25854 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25855 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25856 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25857
25858
25859 final Acceleration invalidGravityNorm = new Acceleration(
25860 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25861
25862 calibrator = null;
25863 try {
25864 calibrator = new KnownGravityNormAccelerometerCalibrator(
25865 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz,
25866 mxy, mxz, myx, myz, mzx, mzy);
25867 fail("IllegalArgumentException expected but not thrown");
25868 } catch (final IllegalArgumentException ignore) {
25869 }
25870 assertNull(calibrator);
25871 }
25872
25873 @Test
25874 public void testConstructor197() throws WrongSizeException {
25875 final Collection<StandardDeviationBodyKinematics> measurements =
25876 Collections.emptyList();
25877
25878 final Matrix ba = generateBa();
25879 final double biasX = ba.getElementAtIndex(0);
25880 final double biasY = ba.getElementAtIndex(1);
25881 final double biasZ = ba.getElementAtIndex(2);
25882
25883 final Matrix ma = generateMaCommonAxis();
25884 final double sx = ma.getElementAt(0, 0);
25885 final double sy = ma.getElementAt(1, 1);
25886 final double sz = ma.getElementAt(2, 2);
25887 final double mxy = ma.getElementAt(0, 1);
25888 final double mxz = ma.getElementAt(0, 2);
25889 final double myx = ma.getElementAt(1, 0);
25890 final double myz = ma.getElementAt(1, 2);
25891 final double mzx = ma.getElementAt(2, 0);
25892 final double mzy = ma.getElementAt(2, 1);
25893
25894 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25895 final double latitude = Math.toRadians(
25896 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25897 final double longitude = Math.toRadians(
25898 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25899 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25900 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25901 final NEDVelocity nedVelocity = new NEDVelocity();
25902 final ECEFPosition ecefPosition = new ECEFPosition();
25903 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25904 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25905 ecefPosition, ecefVelocity);
25906 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25907 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25908 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25909
25910 KnownGravityNormAccelerometerCalibrator calibrator =
25911 new KnownGravityNormAccelerometerCalibrator(
25912 gravityNorm, measurements,
25913 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25914 myx, myz, mzx, mzy);
25915
25916
25917 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25918 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25919 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25920 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25921 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25922 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25923 final Acceleration bx2 = new Acceleration(0.0,
25924 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25925 calibrator.getInitialBiasXAsAcceleration(bx2);
25926 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25927 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25928 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25929 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25930 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25931 final Acceleration by2 = new Acceleration(0.0,
25932 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25933 calibrator.getInitialBiasYAsAcceleration(by2);
25934 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25935 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25936 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25937 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25938 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25939 final Acceleration bz2 = new Acceleration(0.0,
25940 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25941 calibrator.getInitialBiasZAsAcceleration(bz2);
25942 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25943 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25944 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25945 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25946 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25947 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25948 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25949 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25950 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25951 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25952 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25953 final double[] bias1 = calibrator.getInitialBias();
25954 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25955 final double[] bias2 = new double[3];
25956 calibrator.getInitialBias(bias2);
25957 assertArrayEquals(bias1, bias2, 0.0);
25958 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25959 assertEquals(b1, ba);
25960 final Matrix b2 = new Matrix(3, 1);
25961 calibrator.getInitialBiasAsMatrix(b2);
25962 assertEquals(b1, b2);
25963 final Matrix ma1 = new Matrix(3, 3);
25964 ma1.setSubmatrix(0, 0,
25965 2, 2,
25966 new double[]{sx, myx, mzx,
25967 mxy, sy, mzy,
25968 mxz, myz, sz});
25969 assertEquals(calibrator.getInitialMa(), ma1);
25970 final Matrix ma2 = new Matrix(3, 3);
25971 calibrator.getInitialMa(ma2);
25972 assertEquals(ma1, ma2);
25973 assertSame(calibrator.getMeasurements(), measurements);
25974 assertFalse(calibrator.isCommonAxisUsed());
25975 assertNull(calibrator.getListener());
25976 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25977 assertFalse(calibrator.isReady());
25978 assertFalse(calibrator.isRunning());
25979 assertNull(calibrator.getEstimatedBiases());
25980 assertFalse(calibrator.getEstimatedBiases(null));
25981 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25982 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25983 assertNull(calibrator.getEstimatedBiasFx());
25984 assertNull(calibrator.getEstimatedBiasFy());
25985 assertNull(calibrator.getEstimatedBiasFz());
25986 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25987 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25988 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25989 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25990 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25991 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25992 assertNull(calibrator.getEstimatedMa());
25993 assertNull(calibrator.getEstimatedSx());
25994 assertNull(calibrator.getEstimatedSy());
25995 assertNull(calibrator.getEstimatedSz());
25996 assertNull(calibrator.getEstimatedMxy());
25997 assertNull(calibrator.getEstimatedMxz());
25998 assertNull(calibrator.getEstimatedMyx());
25999 assertNull(calibrator.getEstimatedMyz());
26000 assertNull(calibrator.getEstimatedMzx());
26001 assertNull(calibrator.getEstimatedMzy());
26002 assertNull(calibrator.getEstimatedCovariance());
26003 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26004 assertNotNull(calibrator.getGroundTruthGravityNorm());
26005 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26006 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26007 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26008 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26009 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26010 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26011 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26012
26013
26014 final Acceleration invalidGravityNorm = new Acceleration(
26015 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26016
26017 calibrator = null;
26018 try {
26019 calibrator = new KnownGravityNormAccelerometerCalibrator(
26020 invalidGravityNorm, measurements,
26021 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26022 myx, myz, mzx, mzy);
26023 fail("IllegalArgumentException expected but not thrown");
26024 } catch (final IllegalArgumentException ignore) {
26025 }
26026 assertNull(calibrator);
26027 }
26028
26029 @Test
26030 public void testConstructor198() throws WrongSizeException {
26031 final Collection<StandardDeviationBodyKinematics> measurements =
26032 Collections.emptyList();
26033
26034 final Matrix ba = generateBa();
26035 final double biasX = ba.getElementAtIndex(0);
26036 final double biasY = ba.getElementAtIndex(1);
26037 final double biasZ = ba.getElementAtIndex(2);
26038
26039 final Matrix ma = generateMaCommonAxis();
26040 final double sx = ma.getElementAt(0, 0);
26041 final double sy = ma.getElementAt(1, 1);
26042 final double sz = ma.getElementAt(2, 2);
26043 final double mxy = ma.getElementAt(0, 1);
26044 final double mxz = ma.getElementAt(0, 2);
26045 final double myx = ma.getElementAt(1, 0);
26046 final double myz = ma.getElementAt(1, 2);
26047 final double mzx = ma.getElementAt(2, 0);
26048 final double mzy = ma.getElementAt(2, 1);
26049
26050 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26051 final double latitude = Math.toRadians(
26052 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26053 final double longitude = Math.toRadians(
26054 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26055 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26056 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26057 final NEDVelocity nedVelocity = new NEDVelocity();
26058 final ECEFPosition ecefPosition = new ECEFPosition();
26059 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26060 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26061 ecefPosition, ecefVelocity);
26062 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26063 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26064 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26065
26066 KnownGravityNormAccelerometerCalibrator calibrator =
26067 new KnownGravityNormAccelerometerCalibrator(
26068 gravityNorm, measurements,
26069 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26070 myx, myz, mzx, mzy, this);
26071
26072
26073 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26074 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26075 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26076 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26077 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26078 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26079 final Acceleration bx2 = new Acceleration(0.0,
26080 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26081 calibrator.getInitialBiasXAsAcceleration(bx2);
26082 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26083 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26084 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26085 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26086 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26087 final Acceleration by2 = new Acceleration(0.0,
26088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26089 calibrator.getInitialBiasYAsAcceleration(by2);
26090 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26091 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26092 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26093 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26094 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26095 final Acceleration bz2 = new Acceleration(0.0,
26096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26097 calibrator.getInitialBiasZAsAcceleration(bz2);
26098 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26099 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26100 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26101 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26102 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26103 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26104 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26105 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26106 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26107 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26108 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26109 final double[] bias1 = calibrator.getInitialBias();
26110 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26111 final double[] bias2 = new double[3];
26112 calibrator.getInitialBias(bias2);
26113 assertArrayEquals(bias1, bias2, 0.0);
26114 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26115 assertEquals(b1, ba);
26116 final Matrix b2 = new Matrix(3, 1);
26117 calibrator.getInitialBiasAsMatrix(b2);
26118 assertEquals(b1, b2);
26119 final Matrix ma1 = new Matrix(3, 3);
26120 ma1.setSubmatrix(0, 0,
26121 2, 2,
26122 new double[]{sx, myx, mzx,
26123 mxy, sy, mzy,
26124 mxz, myz, sz});
26125 assertEquals(calibrator.getInitialMa(), ma1);
26126 final Matrix ma2 = new Matrix(3, 3);
26127 calibrator.getInitialMa(ma2);
26128 assertEquals(ma1, ma2);
26129 assertSame(calibrator.getMeasurements(), measurements);
26130 assertFalse(calibrator.isCommonAxisUsed());
26131 assertSame(calibrator.getListener(), this);
26132 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26133 assertFalse(calibrator.isReady());
26134 assertFalse(calibrator.isRunning());
26135 assertNull(calibrator.getEstimatedBiases());
26136 assertFalse(calibrator.getEstimatedBiases(null));
26137 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26138 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26139 assertNull(calibrator.getEstimatedBiasFx());
26140 assertNull(calibrator.getEstimatedBiasFy());
26141 assertNull(calibrator.getEstimatedBiasFz());
26142 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26143 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26144 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26145 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26146 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26147 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26148 assertNull(calibrator.getEstimatedMa());
26149 assertNull(calibrator.getEstimatedSx());
26150 assertNull(calibrator.getEstimatedSy());
26151 assertNull(calibrator.getEstimatedSz());
26152 assertNull(calibrator.getEstimatedMxy());
26153 assertNull(calibrator.getEstimatedMxz());
26154 assertNull(calibrator.getEstimatedMyx());
26155 assertNull(calibrator.getEstimatedMyz());
26156 assertNull(calibrator.getEstimatedMzx());
26157 assertNull(calibrator.getEstimatedMzy());
26158 assertNull(calibrator.getEstimatedCovariance());
26159 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26160 assertNotNull(calibrator.getGroundTruthGravityNorm());
26161 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26162 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26163 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26164 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26165 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26166 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26167 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26168
26169
26170 final Acceleration invalidGravityNorm = new Acceleration(
26171 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26172
26173 calibrator = null;
26174 try {
26175 calibrator = new KnownGravityNormAccelerometerCalibrator(
26176 invalidGravityNorm, measurements,
26177 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26178 myx, myz, mzx, mzy, this);
26179 fail("IllegalArgumentException expected but not thrown");
26180 } catch (final IllegalArgumentException ignore) {
26181 }
26182 assertNull(calibrator);
26183 }
26184
26185 @Test
26186 public void testConstructor199() throws WrongSizeException {
26187 final Matrix ba = generateBa();
26188 final double biasX = ba.getElementAtIndex(0);
26189 final double biasY = ba.getElementAtIndex(1);
26190 final double biasZ = ba.getElementAtIndex(2);
26191
26192 final Matrix ma = generateMaCommonAxis();
26193 final double sx = ma.getElementAt(0, 0);
26194 final double sy = ma.getElementAt(1, 1);
26195 final double sz = ma.getElementAt(2, 2);
26196 final double mxy = ma.getElementAt(0, 1);
26197 final double mxz = ma.getElementAt(0, 2);
26198 final double myx = ma.getElementAt(1, 0);
26199 final double myz = ma.getElementAt(1, 2);
26200 final double mzx = ma.getElementAt(2, 0);
26201 final double mzy = ma.getElementAt(2, 1);
26202
26203 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26204 final double latitude = Math.toRadians(
26205 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26206 final double longitude = Math.toRadians(
26207 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26208 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26209 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26210 final NEDVelocity nedVelocity = new NEDVelocity();
26211 final ECEFPosition ecefPosition = new ECEFPosition();
26212 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26213 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26214 ecefPosition, ecefVelocity);
26215 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26216 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26217 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26218
26219 KnownGravityNormAccelerometerCalibrator calibrator =
26220 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26221 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26222 myx, myz, mzx, mzy);
26223
26224
26225 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26226 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26227 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26228 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26229 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26230 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26231 final Acceleration bx2 = new Acceleration(0.0,
26232 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26233 calibrator.getInitialBiasXAsAcceleration(bx2);
26234 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26235 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26236 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26237 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26238 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26239 final Acceleration by2 = new Acceleration(0.0,
26240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26241 calibrator.getInitialBiasYAsAcceleration(by2);
26242 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26243 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26244 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26245 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26246 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26247 final Acceleration bz2 = new Acceleration(0.0,
26248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26249 calibrator.getInitialBiasZAsAcceleration(bz2);
26250 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26251 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26252 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26253 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26254 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26255 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26256 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26257 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26258 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26259 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26260 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26261 final double[] bias1 = calibrator.getInitialBias();
26262 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26263 final double[] bias2 = new double[3];
26264 calibrator.getInitialBias(bias2);
26265 assertArrayEquals(bias1, bias2, 0.0);
26266 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26267 assertEquals(b1, ba);
26268 final Matrix b2 = new Matrix(3, 1);
26269 calibrator.getInitialBiasAsMatrix(b2);
26270 assertEquals(b1, b2);
26271 final Matrix ma1 = new Matrix(3, 3);
26272 ma1.setSubmatrix(0, 0,
26273 2, 2,
26274 new double[]{sx, myx, mzx,
26275 mxy, sy, mzy,
26276 mxz, myz, sz});
26277 assertEquals(calibrator.getInitialMa(), ma1);
26278 final Matrix ma2 = new Matrix(3, 3);
26279 calibrator.getInitialMa(ma2);
26280 assertEquals(ma1, ma2);
26281 assertNull(calibrator.getMeasurements());
26282 assertTrue(calibrator.isCommonAxisUsed());
26283 assertNull(calibrator.getListener());
26284 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26285 assertFalse(calibrator.isReady());
26286 assertFalse(calibrator.isRunning());
26287 assertNull(calibrator.getEstimatedBiases());
26288 assertFalse(calibrator.getEstimatedBiases(null));
26289 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26290 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26291 assertNull(calibrator.getEstimatedBiasFx());
26292 assertNull(calibrator.getEstimatedBiasFy());
26293 assertNull(calibrator.getEstimatedBiasFz());
26294 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26295 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26296 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26297 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26298 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26299 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26300 assertNull(calibrator.getEstimatedMa());
26301 assertNull(calibrator.getEstimatedSx());
26302 assertNull(calibrator.getEstimatedSy());
26303 assertNull(calibrator.getEstimatedSz());
26304 assertNull(calibrator.getEstimatedMxy());
26305 assertNull(calibrator.getEstimatedMxz());
26306 assertNull(calibrator.getEstimatedMyx());
26307 assertNull(calibrator.getEstimatedMyz());
26308 assertNull(calibrator.getEstimatedMzx());
26309 assertNull(calibrator.getEstimatedMzy());
26310 assertNull(calibrator.getEstimatedCovariance());
26311 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26312 assertNotNull(calibrator.getGroundTruthGravityNorm());
26313 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26314 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26315 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26316 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26317 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26318 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26319 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26320
26321
26322 final Acceleration invalidGravityNorm = new Acceleration(
26323 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26324
26325 calibrator = null;
26326 try {
26327 calibrator = new KnownGravityNormAccelerometerCalibrator(
26328 invalidGravityNorm, true, biasX, biasY, biasZ,
26329 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
26330 fail("IllegalArgumentException expected but not thrown");
26331 } catch (final IllegalArgumentException ignore) {
26332 }
26333 assertNull(calibrator);
26334 }
26335
26336 @Test
26337 public void testConstructor200() throws WrongSizeException {
26338 final Matrix ba = generateBa();
26339 final double biasX = ba.getElementAtIndex(0);
26340 final double biasY = ba.getElementAtIndex(1);
26341 final double biasZ = ba.getElementAtIndex(2);
26342
26343 final Matrix ma = generateMaCommonAxis();
26344 final double sx = ma.getElementAt(0, 0);
26345 final double sy = ma.getElementAt(1, 1);
26346 final double sz = ma.getElementAt(2, 2);
26347 final double mxy = ma.getElementAt(0, 1);
26348 final double mxz = ma.getElementAt(0, 2);
26349 final double myx = ma.getElementAt(1, 0);
26350 final double myz = ma.getElementAt(1, 2);
26351 final double mzx = ma.getElementAt(2, 0);
26352 final double mzy = ma.getElementAt(2, 1);
26353
26354 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26355 final double latitude = Math.toRadians(
26356 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26357 final double longitude = Math.toRadians(
26358 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26359 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26360 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26361 final NEDVelocity nedVelocity = new NEDVelocity();
26362 final ECEFPosition ecefPosition = new ECEFPosition();
26363 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26364 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26365 ecefPosition, ecefVelocity);
26366 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26367 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26368 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26369
26370 KnownGravityNormAccelerometerCalibrator calibrator =
26371 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26372 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26373 myx, myz, mzx, mzy, this);
26374
26375
26376 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26377 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26378 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26379 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26380 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26381 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26382 final Acceleration bx2 = new Acceleration(0.0,
26383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26384 calibrator.getInitialBiasXAsAcceleration(bx2);
26385 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26386 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26387 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26388 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26389 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26390 final Acceleration by2 = new Acceleration(0.0,
26391 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26392 calibrator.getInitialBiasYAsAcceleration(by2);
26393 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26394 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26395 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26396 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26397 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26398 final Acceleration bz2 = new Acceleration(0.0,
26399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26400 calibrator.getInitialBiasZAsAcceleration(bz2);
26401 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26402 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26403 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26404 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26405 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26406 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26407 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26408 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26409 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26410 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26411 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26412 final double[] bias1 = calibrator.getInitialBias();
26413 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26414 final double[] bias2 = new double[3];
26415 calibrator.getInitialBias(bias2);
26416 assertArrayEquals(bias1, bias2, 0.0);
26417 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26418 assertEquals(b1, ba);
26419 final Matrix b2 = new Matrix(3, 1);
26420 calibrator.getInitialBiasAsMatrix(b2);
26421 assertEquals(b1, b2);
26422 final Matrix ma1 = new Matrix(3, 3);
26423 ma1.setSubmatrix(0, 0,
26424 2, 2,
26425 new double[]{sx, myx, mzx,
26426 mxy, sy, mzy,
26427 mxz, myz, sz});
26428 assertEquals(calibrator.getInitialMa(), ma1);
26429 final Matrix ma2 = new Matrix(3, 3);
26430 calibrator.getInitialMa(ma2);
26431 assertEquals(ma1, ma2);
26432 assertNull(calibrator.getMeasurements());
26433 assertTrue(calibrator.isCommonAxisUsed());
26434 assertSame(calibrator.getListener(), this);
26435 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26436 assertFalse(calibrator.isReady());
26437 assertFalse(calibrator.isRunning());
26438 assertNull(calibrator.getEstimatedBiases());
26439 assertFalse(calibrator.getEstimatedBiases(null));
26440 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26441 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26442 assertNull(calibrator.getEstimatedBiasFx());
26443 assertNull(calibrator.getEstimatedBiasFy());
26444 assertNull(calibrator.getEstimatedBiasFz());
26445 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26446 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26447 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26448 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26449 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26450 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26451 assertNull(calibrator.getEstimatedMa());
26452 assertNull(calibrator.getEstimatedSx());
26453 assertNull(calibrator.getEstimatedSy());
26454 assertNull(calibrator.getEstimatedSz());
26455 assertNull(calibrator.getEstimatedMxy());
26456 assertNull(calibrator.getEstimatedMxz());
26457 assertNull(calibrator.getEstimatedMyx());
26458 assertNull(calibrator.getEstimatedMyz());
26459 assertNull(calibrator.getEstimatedMzx());
26460 assertNull(calibrator.getEstimatedMzy());
26461 assertNull(calibrator.getEstimatedCovariance());
26462 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26463 assertNotNull(calibrator.getGroundTruthGravityNorm());
26464 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26465 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26466 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26467 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26468 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26469 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26470 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26471
26472
26473 final Acceleration invalidGravityNorm = new Acceleration(
26474 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26475
26476 calibrator = null;
26477 try {
26478 calibrator = new KnownGravityNormAccelerometerCalibrator(
26479 invalidGravityNorm, true, biasX, biasY, biasZ,
26480 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
26481 this);
26482 fail("IllegalArgumentException expected but not thrown");
26483 } catch (final IllegalArgumentException ignore) {
26484 }
26485 assertNull(calibrator);
26486 }
26487
26488 @Test
26489 public void testConstructor201() throws WrongSizeException {
26490 final Collection<StandardDeviationBodyKinematics> measurements =
26491 Collections.emptyList();
26492
26493 final Matrix ba = generateBa();
26494 final double biasX = ba.getElementAtIndex(0);
26495 final double biasY = ba.getElementAtIndex(1);
26496 final double biasZ = ba.getElementAtIndex(2);
26497
26498 final Matrix ma = generateMaCommonAxis();
26499 final double sx = ma.getElementAt(0, 0);
26500 final double sy = ma.getElementAt(1, 1);
26501 final double sz = ma.getElementAt(2, 2);
26502 final double mxy = ma.getElementAt(0, 1);
26503 final double mxz = ma.getElementAt(0, 2);
26504 final double myx = ma.getElementAt(1, 0);
26505 final double myz = ma.getElementAt(1, 2);
26506 final double mzx = ma.getElementAt(2, 0);
26507 final double mzy = ma.getElementAt(2, 1);
26508
26509 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26510 final double latitude = Math.toRadians(
26511 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26512 final double longitude = Math.toRadians(
26513 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26514 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26515 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26516 final NEDVelocity nedVelocity = new NEDVelocity();
26517 final ECEFPosition ecefPosition = new ECEFPosition();
26518 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26519 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26520 ecefPosition, ecefVelocity);
26521 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26522 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26523 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26524
26525 KnownGravityNormAccelerometerCalibrator calibrator =
26526 new KnownGravityNormAccelerometerCalibrator(
26527 gravityNorm, measurements,
26528 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26529 myx, myz, mzx, mzy);
26530
26531
26532 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26533 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26534 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26535 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26536 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26537 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26538 final Acceleration bx2 = new Acceleration(0.0,
26539 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26540 calibrator.getInitialBiasXAsAcceleration(bx2);
26541 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26542 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26543 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26544 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26545 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26546 final Acceleration by2 = new Acceleration(0.0,
26547 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26548 calibrator.getInitialBiasYAsAcceleration(by2);
26549 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26550 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26551 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26552 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26553 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26554 final Acceleration bz2 = new Acceleration(0.0,
26555 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26556 calibrator.getInitialBiasZAsAcceleration(bz2);
26557 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26558 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26559 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26560 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26561 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26562 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26563 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26564 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26565 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26566 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26567 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26568 final double[] bias1 = calibrator.getInitialBias();
26569 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26570 final double[] bias2 = new double[3];
26571 calibrator.getInitialBias(bias2);
26572 assertArrayEquals(bias1, bias2, 0.0);
26573 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26574 assertEquals(b1, ba);
26575 final Matrix b2 = new Matrix(3, 1);
26576 calibrator.getInitialBiasAsMatrix(b2);
26577 assertEquals(b1, b2);
26578 final Matrix ma1 = new Matrix(3, 3);
26579 ma1.setSubmatrix(0, 0,
26580 2, 2,
26581 new double[]{sx, myx, mzx,
26582 mxy, sy, mzy,
26583 mxz, myz, sz});
26584 assertEquals(calibrator.getInitialMa(), ma1);
26585 final Matrix ma2 = new Matrix(3, 3);
26586 calibrator.getInitialMa(ma2);
26587 assertEquals(ma1, ma2);
26588 assertSame(calibrator.getMeasurements(), measurements);
26589 assertTrue(calibrator.isCommonAxisUsed());
26590 assertNull(calibrator.getListener());
26591 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26592 assertFalse(calibrator.isReady());
26593 assertFalse(calibrator.isRunning());
26594 assertNull(calibrator.getEstimatedBiases());
26595 assertFalse(calibrator.getEstimatedBiases(null));
26596 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26597 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26598 assertNull(calibrator.getEstimatedBiasFx());
26599 assertNull(calibrator.getEstimatedBiasFy());
26600 assertNull(calibrator.getEstimatedBiasFz());
26601 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26602 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26603 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26604 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26605 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26606 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26607 assertNull(calibrator.getEstimatedMa());
26608 assertNull(calibrator.getEstimatedSx());
26609 assertNull(calibrator.getEstimatedSy());
26610 assertNull(calibrator.getEstimatedSz());
26611 assertNull(calibrator.getEstimatedMxy());
26612 assertNull(calibrator.getEstimatedMxz());
26613 assertNull(calibrator.getEstimatedMyx());
26614 assertNull(calibrator.getEstimatedMyz());
26615 assertNull(calibrator.getEstimatedMzx());
26616 assertNull(calibrator.getEstimatedMzy());
26617 assertNull(calibrator.getEstimatedCovariance());
26618 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26619 assertNotNull(calibrator.getGroundTruthGravityNorm());
26620 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26621 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26622 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26623 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26624 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26625 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26626 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26627
26628
26629 final Acceleration invalidGravityNorm = new Acceleration(
26630 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26631
26632 calibrator = null;
26633 try {
26634 calibrator = new KnownGravityNormAccelerometerCalibrator(
26635 invalidGravityNorm, measurements,
26636 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26637 myx, myz, mzx, mzy);
26638 fail("IllegalArgumentException expected but not thrown");
26639 } catch (final IllegalArgumentException ignore) {
26640 }
26641 assertNull(calibrator);
26642 }
26643
26644 @Test
26645 public void testConstructor202() throws WrongSizeException {
26646 final Collection<StandardDeviationBodyKinematics> measurements =
26647 Collections.emptyList();
26648
26649 final Matrix ba = generateBa();
26650 final double biasX = ba.getElementAtIndex(0);
26651 final double biasY = ba.getElementAtIndex(1);
26652 final double biasZ = ba.getElementAtIndex(2);
26653
26654 final Matrix ma = generateMaCommonAxis();
26655 final double sx = ma.getElementAt(0, 0);
26656 final double sy = ma.getElementAt(1, 1);
26657 final double sz = ma.getElementAt(2, 2);
26658 final double mxy = ma.getElementAt(0, 1);
26659 final double mxz = ma.getElementAt(0, 2);
26660 final double myx = ma.getElementAt(1, 0);
26661 final double myz = ma.getElementAt(1, 2);
26662 final double mzx = ma.getElementAt(2, 0);
26663 final double mzy = ma.getElementAt(2, 1);
26664
26665 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26666 final double latitude = Math.toRadians(
26667 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26668 final double longitude = Math.toRadians(
26669 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26670 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26671 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26672 final NEDVelocity nedVelocity = new NEDVelocity();
26673 final ECEFPosition ecefPosition = new ECEFPosition();
26674 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26675 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26676 ecefPosition, ecefVelocity);
26677 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26678 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26679 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26680
26681 KnownGravityNormAccelerometerCalibrator calibrator =
26682 new KnownGravityNormAccelerometerCalibrator(
26683 gravityNorm, measurements,
26684 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26685 myx, myz, mzx, mzy, this);
26686
26687
26688 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26689 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26690 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26691 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26692 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26693 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26694 final Acceleration bx2 = new Acceleration(0.0,
26695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26696 calibrator.getInitialBiasXAsAcceleration(bx2);
26697 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26698 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26699 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26700 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26701 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26702 final Acceleration by2 = new Acceleration(0.0,
26703 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26704 calibrator.getInitialBiasYAsAcceleration(by2);
26705 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26706 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26707 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26708 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26709 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26710 final Acceleration bz2 = new Acceleration(0.0,
26711 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26712 calibrator.getInitialBiasZAsAcceleration(bz2);
26713 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26714 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26715 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26716 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26717 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26718 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26719 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26720 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26721 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26722 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26723 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26724 final double[] bias1 = calibrator.getInitialBias();
26725 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26726 final double[] bias2 = new double[3];
26727 calibrator.getInitialBias(bias2);
26728 assertArrayEquals(bias1, bias2, 0.0);
26729 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26730 assertEquals(b1, ba);
26731 final Matrix b2 = new Matrix(3, 1);
26732 calibrator.getInitialBiasAsMatrix(b2);
26733 assertEquals(b1, b2);
26734 final Matrix ma1 = new Matrix(3, 3);
26735 ma1.setSubmatrix(0, 0,
26736 2, 2,
26737 new double[]{sx, myx, mzx,
26738 mxy, sy, mzy,
26739 mxz, myz, sz});
26740 assertEquals(calibrator.getInitialMa(), ma1);
26741 final Matrix ma2 = new Matrix(3, 3);
26742 calibrator.getInitialMa(ma2);
26743 assertEquals(ma1, ma2);
26744 assertSame(calibrator.getMeasurements(), measurements);
26745 assertTrue(calibrator.isCommonAxisUsed());
26746 assertSame(calibrator.getListener(), this);
26747 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26748 assertFalse(calibrator.isReady());
26749 assertFalse(calibrator.isRunning());
26750 assertNull(calibrator.getEstimatedBiases());
26751 assertFalse(calibrator.getEstimatedBiases(null));
26752 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26753 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26754 assertNull(calibrator.getEstimatedBiasFx());
26755 assertNull(calibrator.getEstimatedBiasFy());
26756 assertNull(calibrator.getEstimatedBiasFz());
26757 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26758 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26759 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26760 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26761 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26762 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26763 assertNull(calibrator.getEstimatedMa());
26764 assertNull(calibrator.getEstimatedSx());
26765 assertNull(calibrator.getEstimatedSy());
26766 assertNull(calibrator.getEstimatedSz());
26767 assertNull(calibrator.getEstimatedMxy());
26768 assertNull(calibrator.getEstimatedMxz());
26769 assertNull(calibrator.getEstimatedMyx());
26770 assertNull(calibrator.getEstimatedMyz());
26771 assertNull(calibrator.getEstimatedMzx());
26772 assertNull(calibrator.getEstimatedMzy());
26773 assertNull(calibrator.getEstimatedCovariance());
26774 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26775 assertNotNull(calibrator.getGroundTruthGravityNorm());
26776 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26777 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26778 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26779 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26780 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26781 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26782 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26783
26784
26785 final Acceleration invalidGravityNorm = new Acceleration(
26786 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26787
26788 calibrator = null;
26789 try {
26790 calibrator = new KnownGravityNormAccelerometerCalibrator(
26791 invalidGravityNorm, measurements,
26792 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26793 myx, myz, mzx, mzy, this);
26794 fail("IllegalArgumentException expected but not thrown");
26795 } catch (final IllegalArgumentException ignore) {
26796 }
26797 assertNull(calibrator);
26798 }
26799
26800 @Test
26801 public void testConstructor203() throws WrongSizeException {
26802 final Matrix ba = generateBa();
26803 final double biasX = ba.getElementAtIndex(0);
26804 final double biasY = ba.getElementAtIndex(1);
26805 final double biasZ = ba.getElementAtIndex(2);
26806
26807 final Acceleration bx = new Acceleration(biasX,
26808 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26809 final Acceleration by = new Acceleration(biasY,
26810 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26811 final Acceleration bz = new Acceleration(biasZ,
26812 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26813
26814 final Matrix ma = generateMaCommonAxis();
26815 final double sx = ma.getElementAt(0, 0);
26816 final double sy = ma.getElementAt(1, 1);
26817 final double sz = ma.getElementAt(2, 2);
26818 final double mxy = ma.getElementAt(0, 1);
26819 final double mxz = ma.getElementAt(0, 2);
26820 final double myx = ma.getElementAt(1, 0);
26821 final double myz = ma.getElementAt(1, 2);
26822 final double mzx = ma.getElementAt(2, 0);
26823 final double mzy = ma.getElementAt(2, 1);
26824
26825 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26826 final double latitude = Math.toRadians(
26827 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26828 final double longitude = Math.toRadians(
26829 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26830 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26831 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26832 final NEDVelocity nedVelocity = new NEDVelocity();
26833 final ECEFPosition ecefPosition = new ECEFPosition();
26834 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26835 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26836 ecefPosition, ecefVelocity);
26837 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26838 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26839 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26840
26841 KnownGravityNormAccelerometerCalibrator calibrator =
26842 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26843 bx, by, bz, sx, sy, sz, mxy, mxz,
26844 myx, myz, mzx, mzy);
26845
26846
26847 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26848 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26849 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26850 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26851 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26852 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26853 final Acceleration bx2 = new Acceleration(0.0,
26854 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26855 calibrator.getInitialBiasXAsAcceleration(bx2);
26856 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26857 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26858 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26859 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26860 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26861 final Acceleration by2 = new Acceleration(0.0,
26862 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26863 calibrator.getInitialBiasYAsAcceleration(by2);
26864 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26865 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26866 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26867 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26868 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26869 final Acceleration bz2 = new Acceleration(0.0,
26870 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26871 calibrator.getInitialBiasZAsAcceleration(bz2);
26872 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26873 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26874 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26875 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26876 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26877 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26878 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26879 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26880 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26881 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26882 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26883 final double[] bias1 = calibrator.getInitialBias();
26884 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26885 final double[] bias2 = new double[3];
26886 calibrator.getInitialBias(bias2);
26887 assertArrayEquals(bias1, bias2, 0.0);
26888 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26889 assertEquals(b1, ba);
26890 final Matrix b2 = new Matrix(3, 1);
26891 calibrator.getInitialBiasAsMatrix(b2);
26892 assertEquals(b1, b2);
26893 final Matrix ma1 = new Matrix(3, 3);
26894 ma1.setSubmatrix(0, 0,
26895 2, 2,
26896 new double[]{sx, myx, mzx,
26897 mxy, sy, mzy,
26898 mxz, myz, sz});
26899 assertEquals(calibrator.getInitialMa(), ma1);
26900 final Matrix ma2 = new Matrix(3, 3);
26901 calibrator.getInitialMa(ma2);
26902 assertEquals(ma1, ma2);
26903 assertNull(calibrator.getMeasurements());
26904 assertFalse(calibrator.isCommonAxisUsed());
26905 assertNull(calibrator.getListener());
26906 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26907 assertFalse(calibrator.isReady());
26908 assertFalse(calibrator.isRunning());
26909 assertNull(calibrator.getEstimatedBiases());
26910 assertFalse(calibrator.getEstimatedBiases(null));
26911 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26912 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26913 assertNull(calibrator.getEstimatedBiasFx());
26914 assertNull(calibrator.getEstimatedBiasFy());
26915 assertNull(calibrator.getEstimatedBiasFz());
26916 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26917 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26918 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26919 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26920 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26921 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26922 assertNull(calibrator.getEstimatedMa());
26923 assertNull(calibrator.getEstimatedSx());
26924 assertNull(calibrator.getEstimatedSy());
26925 assertNull(calibrator.getEstimatedSz());
26926 assertNull(calibrator.getEstimatedMxy());
26927 assertNull(calibrator.getEstimatedMxz());
26928 assertNull(calibrator.getEstimatedMyx());
26929 assertNull(calibrator.getEstimatedMyz());
26930 assertNull(calibrator.getEstimatedMzx());
26931 assertNull(calibrator.getEstimatedMzy());
26932 assertNull(calibrator.getEstimatedCovariance());
26933 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26934 assertNotNull(calibrator.getGroundTruthGravityNorm());
26935 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26936 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26937 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26938 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26939 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26940 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26941 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26942
26943
26944 final Acceleration invalidGravityNorm = new Acceleration(
26945 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26946
26947 calibrator = null;
26948 try {
26949 calibrator = new KnownGravityNormAccelerometerCalibrator(
26950 invalidGravityNorm, bx, by, bz, sx, sy, sz,
26951 mxy, mxz, myx, myz, mzx, mzy);
26952 fail("IllegalArgumentException expected but not thrown");
26953 } catch (final IllegalArgumentException ignore) {
26954 }
26955 assertNull(calibrator);
26956 }
26957
26958 @Test
26959 public void testConstructor204() throws WrongSizeException {
26960 final Matrix ba = generateBa();
26961 final double biasX = ba.getElementAtIndex(0);
26962 final double biasY = ba.getElementAtIndex(1);
26963 final double biasZ = ba.getElementAtIndex(2);
26964
26965 final Acceleration bx = new Acceleration(biasX,
26966 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26967 final Acceleration by = new Acceleration(biasY,
26968 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26969 final Acceleration bz = new Acceleration(biasZ,
26970 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26971
26972 final Matrix ma = generateMaCommonAxis();
26973 final double sx = ma.getElementAt(0, 0);
26974 final double sy = ma.getElementAt(1, 1);
26975 final double sz = ma.getElementAt(2, 2);
26976 final double mxy = ma.getElementAt(0, 1);
26977 final double mxz = ma.getElementAt(0, 2);
26978 final double myx = ma.getElementAt(1, 0);
26979 final double myz = ma.getElementAt(1, 2);
26980 final double mzx = ma.getElementAt(2, 0);
26981 final double mzy = ma.getElementAt(2, 1);
26982
26983 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26984 final double latitude = Math.toRadians(
26985 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26986 final double longitude = Math.toRadians(
26987 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26988 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26989 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26990 final NEDVelocity nedVelocity = new NEDVelocity();
26991 final ECEFPosition ecefPosition = new ECEFPosition();
26992 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26993 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26994 ecefPosition, ecefVelocity);
26995 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26996 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26997 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26998
26999 KnownGravityNormAccelerometerCalibrator calibrator =
27000 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27001 bx, by, bz, sx, sy, sz, mxy, mxz,
27002 myx, myz, mzx, mzy, this);
27003
27004
27005 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27006 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27007 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27008 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27009 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27010 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27011 final Acceleration bx2 = new Acceleration(0.0,
27012 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27013 calibrator.getInitialBiasXAsAcceleration(bx2);
27014 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27015 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27016 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27017 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27018 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27019 final Acceleration by2 = new Acceleration(0.0,
27020 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27021 calibrator.getInitialBiasYAsAcceleration(by2);
27022 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27023 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27024 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27025 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27026 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27027 final Acceleration bz2 = new Acceleration(0.0,
27028 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27029 calibrator.getInitialBiasZAsAcceleration(bz2);
27030 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27031 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27032 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27033 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27034 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27035 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27036 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27037 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27038 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27039 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27040 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27041 final double[] bias1 = calibrator.getInitialBias();
27042 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27043 final double[] bias2 = new double[3];
27044 calibrator.getInitialBias(bias2);
27045 assertArrayEquals(bias1, bias2, 0.0);
27046 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27047 assertEquals(b1, ba);
27048 final Matrix b2 = new Matrix(3, 1);
27049 calibrator.getInitialBiasAsMatrix(b2);
27050 assertEquals(b1, b2);
27051 final Matrix ma1 = new Matrix(3, 3);
27052 ma1.setSubmatrix(0, 0,
27053 2, 2,
27054 new double[]{sx, myx, mzx,
27055 mxy, sy, mzy,
27056 mxz, myz, sz});
27057 assertEquals(calibrator.getInitialMa(), ma1);
27058 final Matrix ma2 = new Matrix(3, 3);
27059 calibrator.getInitialMa(ma2);
27060 assertEquals(ma1, ma2);
27061 assertNull(calibrator.getMeasurements());
27062 assertFalse(calibrator.isCommonAxisUsed());
27063 assertSame(calibrator.getListener(), this);
27064 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27065 assertFalse(calibrator.isReady());
27066 assertFalse(calibrator.isRunning());
27067 assertNull(calibrator.getEstimatedBiases());
27068 assertFalse(calibrator.getEstimatedBiases(null));
27069 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27070 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27071 assertNull(calibrator.getEstimatedBiasFx());
27072 assertNull(calibrator.getEstimatedBiasFy());
27073 assertNull(calibrator.getEstimatedBiasFz());
27074 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27075 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27076 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27077 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27078 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27079 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27080 assertNull(calibrator.getEstimatedMa());
27081 assertNull(calibrator.getEstimatedSx());
27082 assertNull(calibrator.getEstimatedSy());
27083 assertNull(calibrator.getEstimatedSz());
27084 assertNull(calibrator.getEstimatedMxy());
27085 assertNull(calibrator.getEstimatedMxz());
27086 assertNull(calibrator.getEstimatedMyx());
27087 assertNull(calibrator.getEstimatedMyz());
27088 assertNull(calibrator.getEstimatedMzx());
27089 assertNull(calibrator.getEstimatedMzy());
27090 assertNull(calibrator.getEstimatedCovariance());
27091 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27092 assertNotNull(calibrator.getGroundTruthGravityNorm());
27093 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27094 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27095 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27096 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27097 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27098 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27099 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27100
27101
27102 final Acceleration invalidGravityNorm = new Acceleration(
27103 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27104
27105 calibrator = null;
27106 try {
27107 calibrator = new KnownGravityNormAccelerometerCalibrator(
27108 invalidGravityNorm, bx, by, bz, sx, sy, sz,
27109 mxy, mxz, myx, myz, mzx, mzy, this);
27110 fail("IllegalArgumentException expected but not thrown");
27111 } catch (final IllegalArgumentException ignore) {
27112 }
27113 assertNull(calibrator);
27114 }
27115
27116 @Test
27117 public void testConstructor205() throws WrongSizeException {
27118 final Collection<StandardDeviationBodyKinematics> measurements =
27119 Collections.emptyList();
27120
27121 final Matrix ba = generateBa();
27122 final double biasX = ba.getElementAtIndex(0);
27123 final double biasY = ba.getElementAtIndex(1);
27124 final double biasZ = ba.getElementAtIndex(2);
27125
27126 final Acceleration bx = new Acceleration(biasX,
27127 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27128 final Acceleration by = new Acceleration(biasY,
27129 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27130 final Acceleration bz = new Acceleration(biasZ,
27131 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27132
27133 final Matrix ma = generateMaCommonAxis();
27134 final double sx = ma.getElementAt(0, 0);
27135 final double sy = ma.getElementAt(1, 1);
27136 final double sz = ma.getElementAt(2, 2);
27137 final double mxy = ma.getElementAt(0, 1);
27138 final double mxz = ma.getElementAt(0, 2);
27139 final double myx = ma.getElementAt(1, 0);
27140 final double myz = ma.getElementAt(1, 2);
27141 final double mzx = ma.getElementAt(2, 0);
27142 final double mzy = ma.getElementAt(2, 1);
27143
27144 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27145 final double latitude = Math.toRadians(
27146 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27147 final double longitude = Math.toRadians(
27148 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27149 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27150 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27151 final NEDVelocity nedVelocity = new NEDVelocity();
27152 final ECEFPosition ecefPosition = new ECEFPosition();
27153 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27154 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27155 ecefPosition, ecefVelocity);
27156 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27157 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27158 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27159
27160 KnownGravityNormAccelerometerCalibrator calibrator =
27161 new KnownGravityNormAccelerometerCalibrator(
27162 gravityNorm, measurements,
27163 bx, by, bz, sx, sy, sz, mxy, mxz,
27164 myx, myz, mzx, mzy);
27165
27166
27167 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27168 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27169 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27170 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27171 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27172 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27173 final Acceleration bx2 = new Acceleration(0.0,
27174 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27175 calibrator.getInitialBiasXAsAcceleration(bx2);
27176 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27177 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27178 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27179 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27180 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27181 final Acceleration by2 = new Acceleration(0.0,
27182 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27183 calibrator.getInitialBiasYAsAcceleration(by2);
27184 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27185 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27186 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27187 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27188 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27189 final Acceleration bz2 = new Acceleration(0.0,
27190 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27191 calibrator.getInitialBiasZAsAcceleration(bz2);
27192 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27193 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27194 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27195 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27196 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27197 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27198 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27199 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27200 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27201 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27202 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27203 final double[] bias1 = calibrator.getInitialBias();
27204 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27205 final double[] bias2 = new double[3];
27206 calibrator.getInitialBias(bias2);
27207 assertArrayEquals(bias1, bias2, 0.0);
27208 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27209 assertEquals(b1, ba);
27210 final Matrix b2 = new Matrix(3, 1);
27211 calibrator.getInitialBiasAsMatrix(b2);
27212 assertEquals(b1, b2);
27213 final Matrix ma1 = new Matrix(3, 3);
27214 ma1.setSubmatrix(0, 0,
27215 2, 2,
27216 new double[]{sx, myx, mzx,
27217 mxy, sy, mzy,
27218 mxz, myz, sz});
27219 assertEquals(calibrator.getInitialMa(), ma1);
27220 final Matrix ma2 = new Matrix(3, 3);
27221 calibrator.getInitialMa(ma2);
27222 assertEquals(ma1, ma2);
27223 assertSame(calibrator.getMeasurements(), measurements);
27224 assertFalse(calibrator.isCommonAxisUsed());
27225 assertNull(calibrator.getListener());
27226 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27227 assertFalse(calibrator.isReady());
27228 assertFalse(calibrator.isRunning());
27229 assertNull(calibrator.getEstimatedBiases());
27230 assertFalse(calibrator.getEstimatedBiases(null));
27231 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27232 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27233 assertNull(calibrator.getEstimatedBiasFx());
27234 assertNull(calibrator.getEstimatedBiasFy());
27235 assertNull(calibrator.getEstimatedBiasFz());
27236 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27237 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27238 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27239 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27240 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27241 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27242 assertNull(calibrator.getEstimatedMa());
27243 assertNull(calibrator.getEstimatedSx());
27244 assertNull(calibrator.getEstimatedSy());
27245 assertNull(calibrator.getEstimatedSz());
27246 assertNull(calibrator.getEstimatedMxy());
27247 assertNull(calibrator.getEstimatedMxz());
27248 assertNull(calibrator.getEstimatedMyx());
27249 assertNull(calibrator.getEstimatedMyz());
27250 assertNull(calibrator.getEstimatedMzx());
27251 assertNull(calibrator.getEstimatedMzy());
27252 assertNull(calibrator.getEstimatedCovariance());
27253 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27254 assertNotNull(calibrator.getGroundTruthGravityNorm());
27255 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27256 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27257 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27258 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27259 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27260 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27261 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27262
27263
27264 final Acceleration invalidGravityNorm = new Acceleration(
27265 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27266
27267 calibrator = null;
27268 try {
27269 calibrator = new KnownGravityNormAccelerometerCalibrator(
27270 invalidGravityNorm, measurements,
27271 bx, by, bz, sx, sy, sz, mxy, mxz,
27272 myx, myz, mzx, mzy);
27273 fail("IllegalArgumentException expected but not thrown");
27274 } catch (final IllegalArgumentException ignore) {
27275 }
27276 assertNull(calibrator);
27277 }
27278
27279 @Test
27280 public void testConstructor206() throws WrongSizeException {
27281 final Collection<StandardDeviationBodyKinematics> measurements =
27282 Collections.emptyList();
27283
27284 final Matrix ba = generateBa();
27285 final double biasX = ba.getElementAtIndex(0);
27286 final double biasY = ba.getElementAtIndex(1);
27287 final double biasZ = ba.getElementAtIndex(2);
27288
27289 final Acceleration bx = new Acceleration(biasX,
27290 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27291 final Acceleration by = new Acceleration(biasY,
27292 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27293 final Acceleration bz = new Acceleration(biasZ,
27294 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27295
27296 final Matrix ma = generateMaCommonAxis();
27297 final double sx = ma.getElementAt(0, 0);
27298 final double sy = ma.getElementAt(1, 1);
27299 final double sz = ma.getElementAt(2, 2);
27300 final double mxy = ma.getElementAt(0, 1);
27301 final double mxz = ma.getElementAt(0, 2);
27302 final double myx = ma.getElementAt(1, 0);
27303 final double myz = ma.getElementAt(1, 2);
27304 final double mzx = ma.getElementAt(2, 0);
27305 final double mzy = ma.getElementAt(2, 1);
27306
27307 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27308 final double latitude = Math.toRadians(
27309 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27310 final double longitude = Math.toRadians(
27311 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27312 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27313 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27314 final NEDVelocity nedVelocity = new NEDVelocity();
27315 final ECEFPosition ecefPosition = new ECEFPosition();
27316 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27317 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27318 ecefPosition, ecefVelocity);
27319 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27320 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27321 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27322
27323 KnownGravityNormAccelerometerCalibrator calibrator =
27324 new KnownGravityNormAccelerometerCalibrator(
27325 gravityNorm, measurements,
27326 bx, by, bz, sx, sy, sz, mxy, mxz,
27327 myx, myz, mzx, mzy, this);
27328
27329
27330 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27331 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27332 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27333 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27334 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27335 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27336 final Acceleration bx2 = new Acceleration(0.0,
27337 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27338 calibrator.getInitialBiasXAsAcceleration(bx2);
27339 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27340 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27341 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27342 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27343 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27344 final Acceleration by2 = new Acceleration(0.0,
27345 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27346 calibrator.getInitialBiasYAsAcceleration(by2);
27347 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27348 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27349 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27350 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27351 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27352 final Acceleration bz2 = new Acceleration(0.0,
27353 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27354 calibrator.getInitialBiasZAsAcceleration(bz2);
27355 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27356 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27357 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27358 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27359 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27360 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27361 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27362 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27363 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27364 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27365 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27366 final double[] bias1 = calibrator.getInitialBias();
27367 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27368 final double[] bias2 = new double[3];
27369 calibrator.getInitialBias(bias2);
27370 assertArrayEquals(bias1, bias2, 0.0);
27371 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27372 assertEquals(b1, ba);
27373 final Matrix b2 = new Matrix(3, 1);
27374 calibrator.getInitialBiasAsMatrix(b2);
27375 assertEquals(b1, b2);
27376 final Matrix ma1 = new Matrix(3, 3);
27377 ma1.setSubmatrix(0, 0,
27378 2, 2,
27379 new double[]{sx, myx, mzx,
27380 mxy, sy, mzy,
27381 mxz, myz, sz});
27382 assertEquals(calibrator.getInitialMa(), ma1);
27383 final Matrix ma2 = new Matrix(3, 3);
27384 calibrator.getInitialMa(ma2);
27385 assertEquals(ma1, ma2);
27386 assertSame(calibrator.getMeasurements(), measurements);
27387 assertFalse(calibrator.isCommonAxisUsed());
27388 assertSame(calibrator.getListener(), this);
27389 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27390 assertFalse(calibrator.isReady());
27391 assertFalse(calibrator.isRunning());
27392 assertNull(calibrator.getEstimatedBiases());
27393 assertFalse(calibrator.getEstimatedBiases(null));
27394 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27395 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27396 assertNull(calibrator.getEstimatedBiasFx());
27397 assertNull(calibrator.getEstimatedBiasFy());
27398 assertNull(calibrator.getEstimatedBiasFz());
27399 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27400 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27401 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27402 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27403 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27404 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27405 assertNull(calibrator.getEstimatedMa());
27406 assertNull(calibrator.getEstimatedSx());
27407 assertNull(calibrator.getEstimatedSy());
27408 assertNull(calibrator.getEstimatedSz());
27409 assertNull(calibrator.getEstimatedMxy());
27410 assertNull(calibrator.getEstimatedMxz());
27411 assertNull(calibrator.getEstimatedMyx());
27412 assertNull(calibrator.getEstimatedMyz());
27413 assertNull(calibrator.getEstimatedMzx());
27414 assertNull(calibrator.getEstimatedMzy());
27415 assertNull(calibrator.getEstimatedCovariance());
27416 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27417 assertNotNull(calibrator.getGroundTruthGravityNorm());
27418 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27419 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27420 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27421 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27422 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27423 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27424 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27425
27426
27427 final Acceleration invalidGravityNorm = new Acceleration(
27428 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27429
27430 calibrator = null;
27431 try {
27432 calibrator = new KnownGravityNormAccelerometerCalibrator(
27433 invalidGravityNorm, measurements,
27434 bx, by, bz, sx, sy, sz, mxy, mxz,
27435 myx, myz, mzx, mzy, this);
27436 fail("IllegalArgumentException expected but not thrown");
27437 } catch (final IllegalArgumentException ignore) {
27438 }
27439 assertNull(calibrator);
27440 }
27441
27442 @Test
27443 public void testConstructor207() throws WrongSizeException {
27444 final Matrix ba = generateBa();
27445 final double biasX = ba.getElementAtIndex(0);
27446 final double biasY = ba.getElementAtIndex(1);
27447 final double biasZ = ba.getElementAtIndex(2);
27448
27449 final Acceleration bx = new Acceleration(biasX,
27450 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27451 final Acceleration by = new Acceleration(biasY,
27452 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27453 final Acceleration bz = new Acceleration(biasZ,
27454 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27455
27456 final Matrix ma = generateMaCommonAxis();
27457 final double sx = ma.getElementAt(0, 0);
27458 final double sy = ma.getElementAt(1, 1);
27459 final double sz = ma.getElementAt(2, 2);
27460 final double mxy = ma.getElementAt(0, 1);
27461 final double mxz = ma.getElementAt(0, 2);
27462 final double myx = ma.getElementAt(1, 0);
27463 final double myz = ma.getElementAt(1, 2);
27464 final double mzx = ma.getElementAt(2, 0);
27465 final double mzy = ma.getElementAt(2, 1);
27466
27467 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27468 final double latitude = Math.toRadians(
27469 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27470 final double longitude = Math.toRadians(
27471 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27472 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27473 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27474 final NEDVelocity nedVelocity = new NEDVelocity();
27475 final ECEFPosition ecefPosition = new ECEFPosition();
27476 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27477 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27478 ecefPosition, ecefVelocity);
27479 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27480 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27481 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27482
27483 KnownGravityNormAccelerometerCalibrator calibrator =
27484 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27485 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27486 myx, myz, mzx, mzy);
27487
27488
27489 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27490 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27491 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27492 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27493 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27494 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27495 final Acceleration bx2 = new Acceleration(0.0,
27496 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27497 calibrator.getInitialBiasXAsAcceleration(bx2);
27498 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27499 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27500 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27501 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27502 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27503 final Acceleration by2 = new Acceleration(0.0,
27504 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27505 calibrator.getInitialBiasYAsAcceleration(by2);
27506 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27507 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27508 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27509 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27510 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27511 final Acceleration bz2 = new Acceleration(0.0,
27512 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27513 calibrator.getInitialBiasZAsAcceleration(bz2);
27514 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27515 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27516 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27517 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27518 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27519 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27520 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27521 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27522 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27523 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27524 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27525 final double[] bias1 = calibrator.getInitialBias();
27526 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27527 final double[] bias2 = new double[3];
27528 calibrator.getInitialBias(bias2);
27529 assertArrayEquals(bias1, bias2, 0.0);
27530 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27531 assertEquals(b1, ba);
27532 final Matrix b2 = new Matrix(3, 1);
27533 calibrator.getInitialBiasAsMatrix(b2);
27534 assertEquals(b1, b2);
27535 final Matrix ma1 = new Matrix(3, 3);
27536 ma1.setSubmatrix(0, 0,
27537 2, 2,
27538 new double[]{sx, myx, mzx,
27539 mxy, sy, mzy,
27540 mxz, myz, sz});
27541 assertEquals(calibrator.getInitialMa(), ma1);
27542 final Matrix ma2 = new Matrix(3, 3);
27543 calibrator.getInitialMa(ma2);
27544 assertEquals(ma1, ma2);
27545 assertNull(calibrator.getMeasurements());
27546 assertTrue(calibrator.isCommonAxisUsed());
27547 assertNull(calibrator.getListener());
27548 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27549 assertFalse(calibrator.isReady());
27550 assertFalse(calibrator.isRunning());
27551 assertNull(calibrator.getEstimatedBiases());
27552 assertFalse(calibrator.getEstimatedBiases(null));
27553 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27554 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27555 assertNull(calibrator.getEstimatedBiasFx());
27556 assertNull(calibrator.getEstimatedBiasFy());
27557 assertNull(calibrator.getEstimatedBiasFz());
27558 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27559 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27560 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27561 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27562 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27563 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27564 assertNull(calibrator.getEstimatedMa());
27565 assertNull(calibrator.getEstimatedSx());
27566 assertNull(calibrator.getEstimatedSy());
27567 assertNull(calibrator.getEstimatedSz());
27568 assertNull(calibrator.getEstimatedMxy());
27569 assertNull(calibrator.getEstimatedMxz());
27570 assertNull(calibrator.getEstimatedMyx());
27571 assertNull(calibrator.getEstimatedMyz());
27572 assertNull(calibrator.getEstimatedMzx());
27573 assertNull(calibrator.getEstimatedMzy());
27574 assertNull(calibrator.getEstimatedCovariance());
27575 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27576 assertNotNull(calibrator.getGroundTruthGravityNorm());
27577 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27578 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27579 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27580 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27581 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27582 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27583 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27584
27585
27586 final Acceleration invalidGravityNorm = new Acceleration(
27587 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27588
27589 calibrator = null;
27590 try {
27591 calibrator = new KnownGravityNormAccelerometerCalibrator(
27592 invalidGravityNorm, true,
27593 bx, by, bz, sx, sy, sz, mxy, mxz,
27594 myx, myz, mzx, mzy);
27595 fail("IllegalArgumentException expected but not thrown");
27596 } catch (final IllegalArgumentException ignore) {
27597 }
27598 assertNull(calibrator);
27599 }
27600
27601 @Test
27602 public void testConstructor208() throws WrongSizeException {
27603 final Matrix ba = generateBa();
27604 final double biasX = ba.getElementAtIndex(0);
27605 final double biasY = ba.getElementAtIndex(1);
27606 final double biasZ = ba.getElementAtIndex(2);
27607
27608 final Acceleration bx = new Acceleration(biasX,
27609 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27610 final Acceleration by = new Acceleration(biasY,
27611 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27612 final Acceleration bz = new Acceleration(biasZ,
27613 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27614
27615 final Matrix ma = generateMaCommonAxis();
27616 final double sx = ma.getElementAt(0, 0);
27617 final double sy = ma.getElementAt(1, 1);
27618 final double sz = ma.getElementAt(2, 2);
27619 final double mxy = ma.getElementAt(0, 1);
27620 final double mxz = ma.getElementAt(0, 2);
27621 final double myx = ma.getElementAt(1, 0);
27622 final double myz = ma.getElementAt(1, 2);
27623 final double mzx = ma.getElementAt(2, 0);
27624 final double mzy = ma.getElementAt(2, 1);
27625
27626 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27627 final double latitude = Math.toRadians(
27628 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27629 final double longitude = Math.toRadians(
27630 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27631 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27632 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27633 final NEDVelocity nedVelocity = new NEDVelocity();
27634 final ECEFPosition ecefPosition = new ECEFPosition();
27635 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27636 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27637 ecefPosition, ecefVelocity);
27638 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27639 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27640 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27641
27642 KnownGravityNormAccelerometerCalibrator calibrator =
27643 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27644 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27645 myx, myz, mzx, mzy, this);
27646
27647
27648 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27649 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27650 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27651 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27652 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27653 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27654 final Acceleration bx2 = new Acceleration(0.0,
27655 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27656 calibrator.getInitialBiasXAsAcceleration(bx2);
27657 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27658 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27659 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27660 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27661 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27662 final Acceleration by2 = new Acceleration(0.0,
27663 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27664 calibrator.getInitialBiasYAsAcceleration(by2);
27665 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27666 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27667 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27668 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27669 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27670 final Acceleration bz2 = new Acceleration(0.0,
27671 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27672 calibrator.getInitialBiasZAsAcceleration(bz2);
27673 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27674 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27675 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27676 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27677 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27678 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27679 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27680 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27681 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27682 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27683 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27684 final double[] bias1 = calibrator.getInitialBias();
27685 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27686 final double[] bias2 = new double[3];
27687 calibrator.getInitialBias(bias2);
27688 assertArrayEquals(bias1, bias2, 0.0);
27689 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27690 assertEquals(b1, ba);
27691 final Matrix b2 = new Matrix(3, 1);
27692 calibrator.getInitialBiasAsMatrix(b2);
27693 assertEquals(b1, b2);
27694 final Matrix ma1 = new Matrix(3, 3);
27695 ma1.setSubmatrix(0, 0,
27696 2, 2,
27697 new double[]{sx, myx, mzx,
27698 mxy, sy, mzy,
27699 mxz, myz, sz});
27700 assertEquals(calibrator.getInitialMa(), ma1);
27701 final Matrix ma2 = new Matrix(3, 3);
27702 calibrator.getInitialMa(ma2);
27703 assertEquals(ma1, ma2);
27704 assertNull(calibrator.getMeasurements());
27705 assertTrue(calibrator.isCommonAxisUsed());
27706 assertSame(calibrator.getListener(), this);
27707 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27708 assertFalse(calibrator.isReady());
27709 assertFalse(calibrator.isRunning());
27710 assertNull(calibrator.getEstimatedBiases());
27711 assertFalse(calibrator.getEstimatedBiases(null));
27712 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27713 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27714 assertNull(calibrator.getEstimatedBiasFx());
27715 assertNull(calibrator.getEstimatedBiasFy());
27716 assertNull(calibrator.getEstimatedBiasFz());
27717 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27718 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27719 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27720 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27721 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27722 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27723 assertNull(calibrator.getEstimatedMa());
27724 assertNull(calibrator.getEstimatedSx());
27725 assertNull(calibrator.getEstimatedSy());
27726 assertNull(calibrator.getEstimatedSz());
27727 assertNull(calibrator.getEstimatedMxy());
27728 assertNull(calibrator.getEstimatedMxz());
27729 assertNull(calibrator.getEstimatedMyx());
27730 assertNull(calibrator.getEstimatedMyz());
27731 assertNull(calibrator.getEstimatedMzx());
27732 assertNull(calibrator.getEstimatedMzy());
27733 assertNull(calibrator.getEstimatedCovariance());
27734 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27735 assertNotNull(calibrator.getGroundTruthGravityNorm());
27736 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27737 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27738 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27739 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27740 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27741 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27742 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27743
27744
27745 final Acceleration invalidGravityNorm = new Acceleration(
27746 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27747
27748 calibrator = null;
27749 try {
27750 calibrator = new KnownGravityNormAccelerometerCalibrator(
27751 invalidGravityNorm, true,
27752 bx, by, bz, sx, sy, sz, mxy, mxz,
27753 myx, myz, mzx, mzy, this);
27754 fail("IllegalArgumentException expected but not thrown");
27755 } catch (final IllegalArgumentException ignore) {
27756 }
27757 assertNull(calibrator);
27758 }
27759
27760 @Test
27761 public void testConstructor209() throws WrongSizeException {
27762 final Collection<StandardDeviationBodyKinematics> measurements =
27763 Collections.emptyList();
27764
27765 final Matrix ba = generateBa();
27766 final double biasX = ba.getElementAtIndex(0);
27767 final double biasY = ba.getElementAtIndex(1);
27768 final double biasZ = ba.getElementAtIndex(2);
27769
27770 final Acceleration bx = new Acceleration(biasX,
27771 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27772 final Acceleration by = new Acceleration(biasY,
27773 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27774 final Acceleration bz = new Acceleration(biasZ,
27775 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27776
27777 final Matrix ma = generateMaCommonAxis();
27778 final double sx = ma.getElementAt(0, 0);
27779 final double sy = ma.getElementAt(1, 1);
27780 final double sz = ma.getElementAt(2, 2);
27781 final double mxy = ma.getElementAt(0, 1);
27782 final double mxz = ma.getElementAt(0, 2);
27783 final double myx = ma.getElementAt(1, 0);
27784 final double myz = ma.getElementAt(1, 2);
27785 final double mzx = ma.getElementAt(2, 0);
27786 final double mzy = ma.getElementAt(2, 1);
27787
27788 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27789 final double latitude = Math.toRadians(
27790 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27791 final double longitude = Math.toRadians(
27792 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27793 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27794 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27795 final NEDVelocity nedVelocity = new NEDVelocity();
27796 final ECEFPosition ecefPosition = new ECEFPosition();
27797 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27798 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27799 ecefPosition, ecefVelocity);
27800 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27801 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27802 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27803
27804 KnownGravityNormAccelerometerCalibrator calibrator =
27805 new KnownGravityNormAccelerometerCalibrator(
27806 gravityNorm, measurements,
27807 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27808 myx, myz, mzx, mzy);
27809
27810
27811 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27812 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27813 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27814 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27815 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27816 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27817 final Acceleration bx2 = new Acceleration(0.0,
27818 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27819 calibrator.getInitialBiasXAsAcceleration(bx2);
27820 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27821 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27822 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27823 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27824 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27825 final Acceleration by2 = new Acceleration(0.0,
27826 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27827 calibrator.getInitialBiasYAsAcceleration(by2);
27828 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27829 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27830 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27831 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27832 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27833 final Acceleration bz2 = new Acceleration(0.0,
27834 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27835 calibrator.getInitialBiasZAsAcceleration(bz2);
27836 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27837 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27838 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27839 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27840 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27841 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27842 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27843 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27844 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27845 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27846 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27847 final double[] bias1 = calibrator.getInitialBias();
27848 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27849 final double[] bias2 = new double[3];
27850 calibrator.getInitialBias(bias2);
27851 assertArrayEquals(bias1, bias2, 0.0);
27852 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27853 assertEquals(b1, ba);
27854 final Matrix b2 = new Matrix(3, 1);
27855 calibrator.getInitialBiasAsMatrix(b2);
27856 assertEquals(b1, b2);
27857 final Matrix ma1 = new Matrix(3, 3);
27858 ma1.setSubmatrix(0, 0,
27859 2, 2,
27860 new double[]{sx, myx, mzx,
27861 mxy, sy, mzy,
27862 mxz, myz, sz});
27863 assertEquals(calibrator.getInitialMa(), ma1);
27864 final Matrix ma2 = new Matrix(3, 3);
27865 calibrator.getInitialMa(ma2);
27866 assertEquals(ma1, ma2);
27867 assertSame(calibrator.getMeasurements(), measurements);
27868 assertTrue(calibrator.isCommonAxisUsed());
27869 assertNull(calibrator.getListener());
27870 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27871 assertFalse(calibrator.isReady());
27872 assertFalse(calibrator.isRunning());
27873 assertNull(calibrator.getEstimatedBiases());
27874 assertFalse(calibrator.getEstimatedBiases(null));
27875 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27876 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27877 assertNull(calibrator.getEstimatedBiasFx());
27878 assertNull(calibrator.getEstimatedBiasFy());
27879 assertNull(calibrator.getEstimatedBiasFz());
27880 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27881 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27882 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27883 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27884 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27885 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27886 assertNull(calibrator.getEstimatedMa());
27887 assertNull(calibrator.getEstimatedSx());
27888 assertNull(calibrator.getEstimatedSy());
27889 assertNull(calibrator.getEstimatedSz());
27890 assertNull(calibrator.getEstimatedMxy());
27891 assertNull(calibrator.getEstimatedMxz());
27892 assertNull(calibrator.getEstimatedMyx());
27893 assertNull(calibrator.getEstimatedMyz());
27894 assertNull(calibrator.getEstimatedMzx());
27895 assertNull(calibrator.getEstimatedMzy());
27896 assertNull(calibrator.getEstimatedCovariance());
27897 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27898 assertNotNull(calibrator.getGroundTruthGravityNorm());
27899 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27900 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27901 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27902 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27903 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27904 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27905 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27906
27907
27908 final Acceleration invalidGravityNorm = new Acceleration(
27909 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27910
27911 calibrator = null;
27912 try {
27913 calibrator = new KnownGravityNormAccelerometerCalibrator(
27914 invalidGravityNorm, measurements,
27915 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27916 myx, myz, mzx, mzy);
27917 fail("IllegalArgumentException expected but not thrown");
27918 } catch (final IllegalArgumentException ignore) {
27919 }
27920 assertNull(calibrator);
27921 }
27922
27923 @Test
27924 public void testConstructor210() throws WrongSizeException {
27925 final Collection<StandardDeviationBodyKinematics> measurements =
27926 Collections.emptyList();
27927
27928 final Matrix ba = generateBa();
27929 final double biasX = ba.getElementAtIndex(0);
27930 final double biasY = ba.getElementAtIndex(1);
27931 final double biasZ = ba.getElementAtIndex(2);
27932
27933 final Acceleration bx = new Acceleration(biasX,
27934 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27935 final Acceleration by = new Acceleration(biasY,
27936 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27937 final Acceleration bz = new Acceleration(biasZ,
27938 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27939
27940 final Matrix ma = generateMaCommonAxis();
27941 final double sx = ma.getElementAt(0, 0);
27942 final double sy = ma.getElementAt(1, 1);
27943 final double sz = ma.getElementAt(2, 2);
27944 final double mxy = ma.getElementAt(0, 1);
27945 final double mxz = ma.getElementAt(0, 2);
27946 final double myx = ma.getElementAt(1, 0);
27947 final double myz = ma.getElementAt(1, 2);
27948 final double mzx = ma.getElementAt(2, 0);
27949 final double mzy = ma.getElementAt(2, 1);
27950
27951 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27952 final double latitude = Math.toRadians(
27953 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27954 final double longitude = Math.toRadians(
27955 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27956 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27957 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27958 final NEDVelocity nedVelocity = new NEDVelocity();
27959 final ECEFPosition ecefPosition = new ECEFPosition();
27960 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27961 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27962 ecefPosition, ecefVelocity);
27963 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27964 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27965 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27966
27967 KnownGravityNormAccelerometerCalibrator calibrator =
27968 new KnownGravityNormAccelerometerCalibrator(
27969 gravityNorm, measurements,
27970 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27971 myx, myz, mzx, mzy, this);
27972
27973
27974 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27975 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27976 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27977 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27978 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27979 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27980 final Acceleration bx2 = new Acceleration(0.0,
27981 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27982 calibrator.getInitialBiasXAsAcceleration(bx2);
27983 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27984 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27985 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27986 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27987 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27988 final Acceleration by2 = new Acceleration(0.0,
27989 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27990 calibrator.getInitialBiasYAsAcceleration(by2);
27991 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27992 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27993 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27994 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27995 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27996 final Acceleration bz2 = new Acceleration(0.0,
27997 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27998 calibrator.getInitialBiasZAsAcceleration(bz2);
27999 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28000 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28001 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28002 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28003 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28004 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28005 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28006 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28007 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28008 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28009 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28010 final double[] bias1 = calibrator.getInitialBias();
28011 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
28012 final double[] bias2 = new double[3];
28013 calibrator.getInitialBias(bias2);
28014 assertArrayEquals(bias1, bias2, 0.0);
28015 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28016 assertEquals(b1, ba);
28017 final Matrix b2 = new Matrix(3, 1);
28018 calibrator.getInitialBiasAsMatrix(b2);
28019 assertEquals(b1, b2);
28020 final Matrix ma1 = new Matrix(3, 3);
28021 ma1.setSubmatrix(0, 0,
28022 2, 2,
28023 new double[]{sx, myx, mzx,
28024 mxy, sy, mzy,
28025 mxz, myz, sz});
28026 assertEquals(calibrator.getInitialMa(), ma1);
28027 final Matrix ma2 = new Matrix(3, 3);
28028 calibrator.getInitialMa(ma2);
28029 assertEquals(ma1, ma2);
28030 assertSame(calibrator.getMeasurements(), measurements);
28031 assertTrue(calibrator.isCommonAxisUsed());
28032 assertSame(calibrator.getListener(), this);
28033 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28034 assertFalse(calibrator.isReady());
28035 assertFalse(calibrator.isRunning());
28036 assertNull(calibrator.getEstimatedBiases());
28037 assertFalse(calibrator.getEstimatedBiases(null));
28038 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28039 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28040 assertNull(calibrator.getEstimatedBiasFx());
28041 assertNull(calibrator.getEstimatedBiasFy());
28042 assertNull(calibrator.getEstimatedBiasFz());
28043 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28044 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28045 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28046 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28047 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28048 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28049 assertNull(calibrator.getEstimatedMa());
28050 assertNull(calibrator.getEstimatedSx());
28051 assertNull(calibrator.getEstimatedSy());
28052 assertNull(calibrator.getEstimatedSz());
28053 assertNull(calibrator.getEstimatedMxy());
28054 assertNull(calibrator.getEstimatedMxz());
28055 assertNull(calibrator.getEstimatedMyx());
28056 assertNull(calibrator.getEstimatedMyz());
28057 assertNull(calibrator.getEstimatedMzx());
28058 assertNull(calibrator.getEstimatedMzy());
28059 assertNull(calibrator.getEstimatedCovariance());
28060 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28061 assertNotNull(calibrator.getGroundTruthGravityNorm());
28062 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28063 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28064 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28065 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28066 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28067 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28068 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28069
28070
28071 final Acceleration invalidGravityNorm = new Acceleration(
28072 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28073
28074 calibrator = null;
28075 try {
28076 calibrator = new KnownGravityNormAccelerometerCalibrator(
28077 invalidGravityNorm, measurements,
28078 true, bx, by, bz, sx, sy, sz, mxy, mxz,
28079 myx, myz, mzx, mzy, this);
28080 fail("IllegalArgumentException expected but not thrown");
28081 } catch (final IllegalArgumentException ignore) {
28082 }
28083 assertNull(calibrator);
28084 }
28085
28086 @Test
28087 public void testConstructor211() throws WrongSizeException {
28088 final Matrix ba = generateBa();
28089 final double[] bias = ba.getBuffer();
28090 final double biasX = ba.getElementAtIndex(0);
28091 final double biasY = ba.getElementAtIndex(1);
28092 final double biasZ = ba.getElementAtIndex(2);
28093
28094 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28095 final double latitude = Math.toRadians(
28096 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28097 final double longitude = Math.toRadians(
28098 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28099 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28100 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28101 final NEDVelocity nedVelocity = new NEDVelocity();
28102 final ECEFPosition ecefPosition = new ECEFPosition();
28103 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28104 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28105 ecefPosition, ecefVelocity);
28106 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28107 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28108 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28109
28110 KnownGravityNormAccelerometerCalibrator calibrator =
28111 new KnownGravityNormAccelerometerCalibrator(
28112 gravityNorm, bias);
28113
28114
28115 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28116 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28117 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28118 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28119 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28120 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28121 final Acceleration bx2 = new Acceleration(0.0,
28122 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28123 calibrator.getInitialBiasXAsAcceleration(bx2);
28124 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28125 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28126 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28127 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28128 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28129 final Acceleration by2 = new Acceleration(0.0,
28130 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28131 calibrator.getInitialBiasYAsAcceleration(by2);
28132 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28133 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28134 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28135 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28136 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28137 final Acceleration bz2 = new Acceleration(0.0,
28138 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28139 calibrator.getInitialBiasZAsAcceleration(bz2);
28140 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28141 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28142 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28143 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28144 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28145 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28146 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28147 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28148 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28149 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28150 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28151 final double[] bias1 = calibrator.getInitialBias();
28152 assertArrayEquals(bias1, bias, 0.0);
28153 final double[] bias2 = new double[3];
28154 calibrator.getInitialBias(bias2);
28155 assertArrayEquals(bias1, bias2, 0.0);
28156 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28157 assertEquals(b1, ba);
28158 final Matrix b2 = new Matrix(3, 1);
28159 calibrator.getInitialBiasAsMatrix(b2);
28160 assertEquals(b1, b2);
28161 final Matrix ma1 = calibrator.getInitialMa();
28162 assertEquals(ma1, new Matrix(3, 3));
28163 final Matrix ma2 = new Matrix(3, 3);
28164 calibrator.getInitialMa(ma2);
28165 assertEquals(ma1, ma2);
28166 assertNull(calibrator.getMeasurements());
28167 assertFalse(calibrator.isCommonAxisUsed());
28168 assertNull(calibrator.getListener());
28169 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28170 assertFalse(calibrator.isReady());
28171 assertFalse(calibrator.isRunning());
28172 assertNull(calibrator.getEstimatedBiases());
28173 assertFalse(calibrator.getEstimatedBiases(null));
28174 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28175 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28176 assertNull(calibrator.getEstimatedBiasFx());
28177 assertNull(calibrator.getEstimatedBiasFy());
28178 assertNull(calibrator.getEstimatedBiasFz());
28179 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28180 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28181 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28182 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28183 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28184 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28185 assertNull(calibrator.getEstimatedMa());
28186 assertNull(calibrator.getEstimatedSx());
28187 assertNull(calibrator.getEstimatedSy());
28188 assertNull(calibrator.getEstimatedSz());
28189 assertNull(calibrator.getEstimatedMxy());
28190 assertNull(calibrator.getEstimatedMxz());
28191 assertNull(calibrator.getEstimatedMyx());
28192 assertNull(calibrator.getEstimatedMyz());
28193 assertNull(calibrator.getEstimatedMzx());
28194 assertNull(calibrator.getEstimatedMzy());
28195 assertNull(calibrator.getEstimatedCovariance());
28196 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28197 assertNotNull(calibrator.getGroundTruthGravityNorm());
28198 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28199 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28200 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28201 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28202 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28203 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28204 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28205
28206
28207 final Acceleration invalidGravityNorm = new Acceleration(
28208 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28209
28210 calibrator = null;
28211 try {
28212 calibrator = new KnownGravityNormAccelerometerCalibrator(
28213 invalidGravityNorm, bias);
28214 fail("IllegalArgumentException expected but not thrown");
28215 } catch (final IllegalArgumentException ignore) {
28216 }
28217 try {
28218 calibrator = new KnownGravityNormAccelerometerCalibrator(
28219 gravityNorm, new double[1]);
28220 fail("IllegalArgumentException expected but not thrown");
28221 } catch (final IllegalArgumentException ignore) {
28222 }
28223 assertNull(calibrator);
28224 }
28225
28226 @Test
28227 public void testConstructor212() throws WrongSizeException {
28228 final Matrix ba = generateBa();
28229 final double[] bias = ba.getBuffer();
28230 final double biasX = ba.getElementAtIndex(0);
28231 final double biasY = ba.getElementAtIndex(1);
28232 final double biasZ = ba.getElementAtIndex(2);
28233
28234 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28235 final double latitude = Math.toRadians(
28236 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28237 final double longitude = Math.toRadians(
28238 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28239 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28240 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28241 final NEDVelocity nedVelocity = new NEDVelocity();
28242 final ECEFPosition ecefPosition = new ECEFPosition();
28243 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28244 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28245 ecefPosition, ecefVelocity);
28246 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28247 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28248 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28249
28250 KnownGravityNormAccelerometerCalibrator calibrator =
28251 new KnownGravityNormAccelerometerCalibrator(
28252 gravityNorm, bias, this);
28253
28254
28255 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28256 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28257 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28258 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28259 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28260 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28261 final Acceleration bx2 = new Acceleration(0.0,
28262 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28263 calibrator.getInitialBiasXAsAcceleration(bx2);
28264 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28265 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28266 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28267 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28268 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28269 final Acceleration by2 = new Acceleration(0.0,
28270 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28271 calibrator.getInitialBiasYAsAcceleration(by2);
28272 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28273 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28274 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28275 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28276 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28277 final Acceleration bz2 = new Acceleration(0.0,
28278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28279 calibrator.getInitialBiasZAsAcceleration(bz2);
28280 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28281 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28283 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28284 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28285 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28286 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28287 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28288 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28289 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28290 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28291 final double[] bias1 = calibrator.getInitialBias();
28292 assertArrayEquals(bias1, bias, 0.0);
28293 final double[] bias2 = new double[3];
28294 calibrator.getInitialBias(bias2);
28295 assertArrayEquals(bias1, bias2, 0.0);
28296 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28297 assertEquals(b1, ba);
28298 final Matrix b2 = new Matrix(3, 1);
28299 calibrator.getInitialBiasAsMatrix(b2);
28300 assertEquals(b1, b2);
28301 final Matrix ma1 = calibrator.getInitialMa();
28302 assertEquals(ma1, new Matrix(3, 3));
28303 final Matrix ma2 = new Matrix(3, 3);
28304 calibrator.getInitialMa(ma2);
28305 assertEquals(ma1, ma2);
28306 assertNull(calibrator.getMeasurements());
28307 assertFalse(calibrator.isCommonAxisUsed());
28308 assertSame(calibrator.getListener(), this);
28309 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28310 assertFalse(calibrator.isReady());
28311 assertFalse(calibrator.isRunning());
28312 assertNull(calibrator.getEstimatedBiases());
28313 assertFalse(calibrator.getEstimatedBiases(null));
28314 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28315 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28316 assertNull(calibrator.getEstimatedBiasFx());
28317 assertNull(calibrator.getEstimatedBiasFy());
28318 assertNull(calibrator.getEstimatedBiasFz());
28319 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28320 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28321 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28322 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28323 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28324 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28325 assertNull(calibrator.getEstimatedMa());
28326 assertNull(calibrator.getEstimatedSx());
28327 assertNull(calibrator.getEstimatedSy());
28328 assertNull(calibrator.getEstimatedSz());
28329 assertNull(calibrator.getEstimatedMxy());
28330 assertNull(calibrator.getEstimatedMxz());
28331 assertNull(calibrator.getEstimatedMyx());
28332 assertNull(calibrator.getEstimatedMyz());
28333 assertNull(calibrator.getEstimatedMzx());
28334 assertNull(calibrator.getEstimatedMzy());
28335 assertNull(calibrator.getEstimatedCovariance());
28336 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28337 assertNotNull(calibrator.getGroundTruthGravityNorm());
28338 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28339 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28340 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28341 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28342 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28343 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28344 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28345
28346
28347 final Acceleration invalidGravityNorm = new Acceleration(
28348 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28349
28350 calibrator = null;
28351 try {
28352 calibrator = new KnownGravityNormAccelerometerCalibrator(
28353 invalidGravityNorm, bias, this);
28354 fail("IllegalArgumentException expected but not thrown");
28355 } catch (final IllegalArgumentException ignore) {
28356 }
28357 try {
28358 calibrator = new KnownGravityNormAccelerometerCalibrator(
28359 gravityNorm, new double[1], this);
28360 fail("IllegalArgumentException expected but not thrown");
28361 } catch (final IllegalArgumentException ignore) {
28362 }
28363 assertNull(calibrator);
28364 }
28365
28366 @Test
28367 public void testConstructor213() throws WrongSizeException {
28368 final Collection<StandardDeviationBodyKinematics> measurements =
28369 Collections.emptyList();
28370
28371 final Matrix ba = generateBa();
28372 final double[] bias = ba.getBuffer();
28373 final double biasX = ba.getElementAtIndex(0);
28374 final double biasY = ba.getElementAtIndex(1);
28375 final double biasZ = ba.getElementAtIndex(2);
28376
28377 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28378 final double latitude = Math.toRadians(
28379 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28380 final double longitude = Math.toRadians(
28381 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28382 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28383 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28384 final NEDVelocity nedVelocity = new NEDVelocity();
28385 final ECEFPosition ecefPosition = new ECEFPosition();
28386 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28387 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28388 ecefPosition, ecefVelocity);
28389 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28390 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28391 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28392
28393 KnownGravityNormAccelerometerCalibrator calibrator =
28394 new KnownGravityNormAccelerometerCalibrator(
28395 gravityNorm, measurements, bias);
28396
28397
28398 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28399 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28400 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28401 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28402 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28403 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28404 final Acceleration bx2 = new Acceleration(0.0,
28405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28406 calibrator.getInitialBiasXAsAcceleration(bx2);
28407 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28408 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28409 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28410 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28411 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28412 final Acceleration by2 = new Acceleration(0.0,
28413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28414 calibrator.getInitialBiasYAsAcceleration(by2);
28415 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28416 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28417 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28418 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28419 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28420 final Acceleration bz2 = new Acceleration(0.0,
28421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28422 calibrator.getInitialBiasZAsAcceleration(bz2);
28423 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28424 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28425 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28426 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28427 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28428 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28429 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28430 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28431 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28432 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28433 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28434 final double[] bias1 = calibrator.getInitialBias();
28435 assertArrayEquals(bias1, bias, 0.0);
28436 final double[] bias2 = new double[3];
28437 calibrator.getInitialBias(bias2);
28438 assertArrayEquals(bias1, bias2, 0.0);
28439 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28440 assertEquals(b1, ba);
28441 final Matrix b2 = new Matrix(3, 1);
28442 calibrator.getInitialBiasAsMatrix(b2);
28443 assertEquals(b1, b2);
28444 final Matrix ma1 = calibrator.getInitialMa();
28445 assertEquals(ma1, new Matrix(3, 3));
28446 final Matrix ma2 = new Matrix(3, 3);
28447 calibrator.getInitialMa(ma2);
28448 assertEquals(ma1, ma2);
28449 assertSame(calibrator.getMeasurements(), measurements);
28450 assertFalse(calibrator.isCommonAxisUsed());
28451 assertNull(calibrator.getListener());
28452 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28453 assertFalse(calibrator.isReady());
28454 assertFalse(calibrator.isRunning());
28455 assertNull(calibrator.getEstimatedBiases());
28456 assertFalse(calibrator.getEstimatedBiases(null));
28457 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28458 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28459 assertNull(calibrator.getEstimatedBiasFx());
28460 assertNull(calibrator.getEstimatedBiasFy());
28461 assertNull(calibrator.getEstimatedBiasFz());
28462 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28463 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28464 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28465 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28466 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28467 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28468 assertNull(calibrator.getEstimatedMa());
28469 assertNull(calibrator.getEstimatedSx());
28470 assertNull(calibrator.getEstimatedSy());
28471 assertNull(calibrator.getEstimatedSz());
28472 assertNull(calibrator.getEstimatedMxy());
28473 assertNull(calibrator.getEstimatedMxz());
28474 assertNull(calibrator.getEstimatedMyx());
28475 assertNull(calibrator.getEstimatedMyz());
28476 assertNull(calibrator.getEstimatedMzx());
28477 assertNull(calibrator.getEstimatedMzy());
28478 assertNull(calibrator.getEstimatedCovariance());
28479 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28480 assertNotNull(calibrator.getGroundTruthGravityNorm());
28481 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28482 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28483 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28484 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28485 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28486 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28487 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28488
28489
28490 final Acceleration invalidGravityNorm = new Acceleration(
28491 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28492
28493 calibrator = null;
28494 try {
28495 calibrator = new KnownGravityNormAccelerometerCalibrator(
28496 invalidGravityNorm, measurements, bias);
28497 fail("IllegalArgumentException expected but not thrown");
28498 } catch (final IllegalArgumentException ignore) {
28499 }
28500 try {
28501 calibrator = new KnownGravityNormAccelerometerCalibrator(
28502 gravityNorm, measurements, new double[1]);
28503 fail("IllegalArgumentException expected but not thrown");
28504 } catch (final IllegalArgumentException ignore) {
28505 }
28506 assertNull(calibrator);
28507 }
28508
28509 @Test
28510 public void testConstructor214() throws WrongSizeException {
28511 final Collection<StandardDeviationBodyKinematics> measurements =
28512 Collections.emptyList();
28513
28514 final Matrix ba = generateBa();
28515 final double[] bias = ba.getBuffer();
28516 final double biasX = ba.getElementAtIndex(0);
28517 final double biasY = ba.getElementAtIndex(1);
28518 final double biasZ = ba.getElementAtIndex(2);
28519
28520 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28521 final double latitude = Math.toRadians(
28522 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28523 final double longitude = Math.toRadians(
28524 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28525 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28526 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28527 final NEDVelocity nedVelocity = new NEDVelocity();
28528 final ECEFPosition ecefPosition = new ECEFPosition();
28529 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28530 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28531 ecefPosition, ecefVelocity);
28532 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28533 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28534 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28535
28536 KnownGravityNormAccelerometerCalibrator calibrator =
28537 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
28538 measurements, bias, this);
28539
28540
28541 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28542 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28543 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28544 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28545 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28547 final Acceleration bx2 = new Acceleration(0.0,
28548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28549 calibrator.getInitialBiasXAsAcceleration(bx2);
28550 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28552 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28553 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28555 final Acceleration by2 = new Acceleration(0.0,
28556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28557 calibrator.getInitialBiasYAsAcceleration(by2);
28558 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28560 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28561 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28563 final Acceleration bz2 = new Acceleration(0.0,
28564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28565 calibrator.getInitialBiasZAsAcceleration(bz2);
28566 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28568 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28569 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28570 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28571 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28572 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28573 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28574 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28575 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28576 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28577 final double[] bias1 = calibrator.getInitialBias();
28578 assertArrayEquals(bias1, bias, 0.0);
28579 final double[] bias2 = new double[3];
28580 calibrator.getInitialBias(bias2);
28581 assertArrayEquals(bias1, bias2, 0.0);
28582 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28583 assertEquals(b1, ba);
28584 final Matrix b2 = new Matrix(3, 1);
28585 calibrator.getInitialBiasAsMatrix(b2);
28586 assertEquals(b1, b2);
28587 final Matrix ma1 = calibrator.getInitialMa();
28588 assertEquals(ma1, new Matrix(3, 3));
28589 final Matrix ma2 = new Matrix(3, 3);
28590 calibrator.getInitialMa(ma2);
28591 assertEquals(ma1, ma2);
28592 assertSame(calibrator.getMeasurements(), measurements);
28593 assertFalse(calibrator.isCommonAxisUsed());
28594 assertSame(calibrator.getListener(), this);
28595 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28596 assertFalse(calibrator.isReady());
28597 assertFalse(calibrator.isRunning());
28598 assertNull(calibrator.getEstimatedBiases());
28599 assertFalse(calibrator.getEstimatedBiases(null));
28600 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28601 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28602 assertNull(calibrator.getEstimatedBiasFx());
28603 assertNull(calibrator.getEstimatedBiasFy());
28604 assertNull(calibrator.getEstimatedBiasFz());
28605 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28606 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28607 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28608 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28609 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28610 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28611 assertNull(calibrator.getEstimatedMa());
28612 assertNull(calibrator.getEstimatedSx());
28613 assertNull(calibrator.getEstimatedSy());
28614 assertNull(calibrator.getEstimatedSz());
28615 assertNull(calibrator.getEstimatedMxy());
28616 assertNull(calibrator.getEstimatedMxz());
28617 assertNull(calibrator.getEstimatedMyx());
28618 assertNull(calibrator.getEstimatedMyz());
28619 assertNull(calibrator.getEstimatedMzx());
28620 assertNull(calibrator.getEstimatedMzy());
28621 assertNull(calibrator.getEstimatedCovariance());
28622 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28623 assertNotNull(calibrator.getGroundTruthGravityNorm());
28624 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28625 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28626 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28627 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28628 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28629 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28630 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28631
28632
28633 final Acceleration invalidGravityNorm = new Acceleration(
28634 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28635
28636 calibrator = null;
28637 try {
28638 calibrator = new KnownGravityNormAccelerometerCalibrator(
28639 invalidGravityNorm, measurements, bias, this);
28640 fail("IllegalArgumentException expected but not thrown");
28641 } catch (final IllegalArgumentException ignore) {
28642 }
28643 try {
28644 calibrator = new KnownGravityNormAccelerometerCalibrator(
28645 gravityNorm, measurements,
28646 new double[1], this);
28647 fail("IllegalArgumentException expected but not thrown");
28648 } catch (final IllegalArgumentException ignore) {
28649 }
28650 assertNull(calibrator);
28651 }
28652
28653 @Test
28654 public void testConstructor215() throws WrongSizeException {
28655 final Matrix ba = generateBa();
28656 final double[] bias = ba.getBuffer();
28657 final double biasX = ba.getElementAtIndex(0);
28658 final double biasY = ba.getElementAtIndex(1);
28659 final double biasZ = ba.getElementAtIndex(2);
28660
28661 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28662 final double latitude = Math.toRadians(
28663 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28664 final double longitude = Math.toRadians(
28665 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28666 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28667 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28668 final NEDVelocity nedVelocity = new NEDVelocity();
28669 final ECEFPosition ecefPosition = new ECEFPosition();
28670 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28671 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28672 ecefPosition, ecefVelocity);
28673 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28674 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28675 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28676
28677 KnownGravityNormAccelerometerCalibrator calibrator =
28678 new KnownGravityNormAccelerometerCalibrator(
28679 gravityNorm, true, bias);
28680
28681
28682 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28683 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28684 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28685 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28686 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28687 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28688 final Acceleration bx2 = new Acceleration(0.0,
28689 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28690 calibrator.getInitialBiasXAsAcceleration(bx2);
28691 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28692 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28693 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28694 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28695 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28696 final Acceleration by2 = new Acceleration(0.0,
28697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28698 calibrator.getInitialBiasYAsAcceleration(by2);
28699 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28700 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28701 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28702 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28703 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28704 final Acceleration bz2 = new Acceleration(0.0,
28705 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28706 calibrator.getInitialBiasZAsAcceleration(bz2);
28707 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28708 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28709 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28710 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28711 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28712 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28713 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28714 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28715 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28716 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28717 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28718 final double[] bias1 = calibrator.getInitialBias();
28719 assertArrayEquals(bias1, bias, 0.0);
28720 final double[] bias2 = new double[3];
28721 calibrator.getInitialBias(bias2);
28722 assertArrayEquals(bias1, bias2, 0.0);
28723 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28724 assertEquals(b1, ba);
28725 final Matrix b2 = new Matrix(3, 1);
28726 calibrator.getInitialBiasAsMatrix(b2);
28727 assertEquals(b1, b2);
28728 final Matrix ma1 = calibrator.getInitialMa();
28729 assertEquals(ma1, new Matrix(3, 3));
28730 final Matrix ma2 = new Matrix(3, 3);
28731 calibrator.getInitialMa(ma2);
28732 assertEquals(ma1, ma2);
28733 assertNull(calibrator.getMeasurements());
28734 assertTrue(calibrator.isCommonAxisUsed());
28735 assertNull(calibrator.getListener());
28736 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28737 assertFalse(calibrator.isReady());
28738 assertFalse(calibrator.isRunning());
28739 assertNull(calibrator.getEstimatedBiases());
28740 assertFalse(calibrator.getEstimatedBiases(null));
28741 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28742 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28743 assertNull(calibrator.getEstimatedBiasFx());
28744 assertNull(calibrator.getEstimatedBiasFy());
28745 assertNull(calibrator.getEstimatedBiasFz());
28746 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28747 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28748 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28749 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28750 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28751 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28752 assertNull(calibrator.getEstimatedMa());
28753 assertNull(calibrator.getEstimatedSx());
28754 assertNull(calibrator.getEstimatedSy());
28755 assertNull(calibrator.getEstimatedSz());
28756 assertNull(calibrator.getEstimatedMxy());
28757 assertNull(calibrator.getEstimatedMxz());
28758 assertNull(calibrator.getEstimatedMyx());
28759 assertNull(calibrator.getEstimatedMyz());
28760 assertNull(calibrator.getEstimatedMzx());
28761 assertNull(calibrator.getEstimatedMzy());
28762 assertNull(calibrator.getEstimatedCovariance());
28763 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28764 assertNotNull(calibrator.getGroundTruthGravityNorm());
28765 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28766 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28767 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28768 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28769 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28770 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28771 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28772
28773
28774 final Acceleration invalidGravityNorm = new Acceleration(
28775 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28776
28777 calibrator = null;
28778 try {
28779 calibrator = new KnownGravityNormAccelerometerCalibrator(
28780 invalidGravityNorm, true, bias);
28781 fail("IllegalArgumentException expected but not thrown");
28782 } catch (final IllegalArgumentException ignore) {
28783 }
28784 try {
28785 calibrator = new KnownGravityNormAccelerometerCalibrator(
28786 gravityNorm, true, new double[1]);
28787 fail("IllegalArgumentException expected but not thrown");
28788 } catch (final IllegalArgumentException ignore) {
28789 }
28790 assertNull(calibrator);
28791 }
28792
28793 @Test
28794 public void testConstructor216() throws WrongSizeException {
28795 final Matrix ba = generateBa();
28796 final double[] bias = ba.getBuffer();
28797 final double biasX = ba.getElementAtIndex(0);
28798 final double biasY = ba.getElementAtIndex(1);
28799 final double biasZ = ba.getElementAtIndex(2);
28800
28801 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28802 final double latitude = Math.toRadians(
28803 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28804 final double longitude = Math.toRadians(
28805 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28806 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28807 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28808 final NEDVelocity nedVelocity = new NEDVelocity();
28809 final ECEFPosition ecefPosition = new ECEFPosition();
28810 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28811 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28812 ecefPosition, ecefVelocity);
28813 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28814 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28815 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28816
28817 KnownGravityNormAccelerometerCalibrator calibrator =
28818 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
28819 true, bias, this);
28820
28821
28822 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28823 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28824 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28825 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28826 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28827 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28828 final Acceleration bx2 = new Acceleration(0.0,
28829 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28830 calibrator.getInitialBiasXAsAcceleration(bx2);
28831 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28832 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28833 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28834 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28835 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28836 final Acceleration by2 = new Acceleration(0.0,
28837 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28838 calibrator.getInitialBiasYAsAcceleration(by2);
28839 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28840 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28841 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28842 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28843 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28844 final Acceleration bz2 = new Acceleration(0.0,
28845 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28846 calibrator.getInitialBiasZAsAcceleration(bz2);
28847 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28848 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28849 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28850 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28851 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28852 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28853 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28854 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28855 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28856 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28857 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28858 final double[] bias1 = calibrator.getInitialBias();
28859 assertArrayEquals(bias1, bias, 0.0);
28860 final double[] bias2 = new double[3];
28861 calibrator.getInitialBias(bias2);
28862 assertArrayEquals(bias1, bias2, 0.0);
28863 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28864 assertEquals(b1, ba);
28865 final Matrix b2 = new Matrix(3, 1);
28866 calibrator.getInitialBiasAsMatrix(b2);
28867 assertEquals(b1, b2);
28868 final Matrix ma1 = calibrator.getInitialMa();
28869 assertEquals(ma1, new Matrix(3, 3));
28870 final Matrix ma2 = new Matrix(3, 3);
28871 calibrator.getInitialMa(ma2);
28872 assertEquals(ma1, ma2);
28873 assertNull(calibrator.getMeasurements());
28874 assertTrue(calibrator.isCommonAxisUsed());
28875 assertSame(calibrator.getListener(), this);
28876 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28877 assertFalse(calibrator.isReady());
28878 assertFalse(calibrator.isRunning());
28879 assertNull(calibrator.getEstimatedBiases());
28880 assertFalse(calibrator.getEstimatedBiases(null));
28881 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28882 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28883 assertNull(calibrator.getEstimatedBiasFx());
28884 assertNull(calibrator.getEstimatedBiasFy());
28885 assertNull(calibrator.getEstimatedBiasFz());
28886 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28887 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28888 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28889 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28890 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28891 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28892 assertNull(calibrator.getEstimatedMa());
28893 assertNull(calibrator.getEstimatedSx());
28894 assertNull(calibrator.getEstimatedSy());
28895 assertNull(calibrator.getEstimatedSz());
28896 assertNull(calibrator.getEstimatedMxy());
28897 assertNull(calibrator.getEstimatedMxz());
28898 assertNull(calibrator.getEstimatedMyx());
28899 assertNull(calibrator.getEstimatedMyz());
28900 assertNull(calibrator.getEstimatedMzx());
28901 assertNull(calibrator.getEstimatedMzy());
28902 assertNull(calibrator.getEstimatedCovariance());
28903 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28904 assertNotNull(calibrator.getGroundTruthGravityNorm());
28905 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28906 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28907 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28908 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28909 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28910 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28911 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28912
28913
28914 final Acceleration invalidGravityNorm = new Acceleration(
28915 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28916
28917 calibrator = null;
28918 try {
28919 calibrator = new KnownGravityNormAccelerometerCalibrator(
28920 invalidGravityNorm, true,
28921 bias, this);
28922 fail("IllegalArgumentException expected but not thrown");
28923 } catch (final IllegalArgumentException ignore) {
28924 }
28925 try {
28926 calibrator = new KnownGravityNormAccelerometerCalibrator(
28927 gravityNorm, true,
28928 new double[1], this);
28929 fail("IllegalArgumentException expected but not thrown");
28930 } catch (final IllegalArgumentException ignore) {
28931 }
28932 assertNull(calibrator);
28933 }
28934
28935 @Test
28936 public void testConstructor217() throws WrongSizeException {
28937 final Collection<StandardDeviationBodyKinematics> measurements =
28938 Collections.emptyList();
28939
28940 final Matrix ba = generateBa();
28941 final double[] bias = ba.getBuffer();
28942 final double biasX = ba.getElementAtIndex(0);
28943 final double biasY = ba.getElementAtIndex(1);
28944 final double biasZ = ba.getElementAtIndex(2);
28945
28946 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28947 final double latitude = Math.toRadians(
28948 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28949 final double longitude = Math.toRadians(
28950 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28951 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28952 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28953 final NEDVelocity nedVelocity = new NEDVelocity();
28954 final ECEFPosition ecefPosition = new ECEFPosition();
28955 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28956 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28957 ecefPosition, ecefVelocity);
28958 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28959 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28960 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28961
28962 KnownGravityNormAccelerometerCalibrator calibrator =
28963 new KnownGravityNormAccelerometerCalibrator(
28964 gravityNorm, measurements,
28965 true, bias);
28966
28967
28968 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28969 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28970 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28971 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28972 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28973 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28974 final Acceleration bx2 = new Acceleration(0.0,
28975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28976 calibrator.getInitialBiasXAsAcceleration(bx2);
28977 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28978 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28979 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28980 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28981 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28982 final Acceleration by2 = new Acceleration(0.0,
28983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28984 calibrator.getInitialBiasYAsAcceleration(by2);
28985 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28986 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28987 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28988 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28989 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28990 final Acceleration bz2 = new Acceleration(0.0,
28991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28992 calibrator.getInitialBiasZAsAcceleration(bz2);
28993 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28994 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28995 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28996 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28997 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28998 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28999 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29000 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29001 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29002 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29003 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29004 final double[] bias1 = calibrator.getInitialBias();
29005 assertArrayEquals(bias1, bias, 0.0);
29006 final double[] bias2 = new double[3];
29007 calibrator.getInitialBias(bias2);
29008 assertArrayEquals(bias1, bias2, 0.0);
29009 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29010 assertEquals(b1, ba);
29011 final Matrix b2 = new Matrix(3, 1);
29012 calibrator.getInitialBiasAsMatrix(b2);
29013 assertEquals(b1, b2);
29014 final Matrix ma1 = calibrator.getInitialMa();
29015 assertEquals(ma1, new Matrix(3, 3));
29016 final Matrix ma2 = new Matrix(3, 3);
29017 calibrator.getInitialMa(ma2);
29018 assertEquals(ma1, ma2);
29019 assertSame(calibrator.getMeasurements(), measurements);
29020 assertTrue(calibrator.isCommonAxisUsed());
29021 assertNull(calibrator.getListener());
29022 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29023 assertFalse(calibrator.isReady());
29024 assertFalse(calibrator.isRunning());
29025 assertNull(calibrator.getEstimatedBiases());
29026 assertFalse(calibrator.getEstimatedBiases(null));
29027 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29028 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29029 assertNull(calibrator.getEstimatedBiasFx());
29030 assertNull(calibrator.getEstimatedBiasFy());
29031 assertNull(calibrator.getEstimatedBiasFz());
29032 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29033 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29034 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29035 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29036 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29037 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29038 assertNull(calibrator.getEstimatedMa());
29039 assertNull(calibrator.getEstimatedSx());
29040 assertNull(calibrator.getEstimatedSy());
29041 assertNull(calibrator.getEstimatedSz());
29042 assertNull(calibrator.getEstimatedMxy());
29043 assertNull(calibrator.getEstimatedMxz());
29044 assertNull(calibrator.getEstimatedMyx());
29045 assertNull(calibrator.getEstimatedMyz());
29046 assertNull(calibrator.getEstimatedMzx());
29047 assertNull(calibrator.getEstimatedMzy());
29048 assertNull(calibrator.getEstimatedCovariance());
29049 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29050 assertNotNull(calibrator.getGroundTruthGravityNorm());
29051 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29052 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29053 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29054 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29055 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29056 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29057 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29058
29059
29060 final Acceleration invalidGravityNorm = new Acceleration(
29061 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29062
29063 calibrator = null;
29064 try {
29065 calibrator = new KnownGravityNormAccelerometerCalibrator(
29066 invalidGravityNorm, measurements,
29067 true, bias);
29068 fail("IllegalArgumentException expected but not thrown");
29069 } catch (final IllegalArgumentException ignore) {
29070 }
29071 try {
29072 calibrator = new KnownGravityNormAccelerometerCalibrator(
29073 gravityNorm, measurements,
29074 true, new double[1]);
29075 fail("IllegalArgumentException expected but not thrown");
29076 } catch (final IllegalArgumentException ignore) {
29077 }
29078 assertNull(calibrator);
29079 }
29080
29081 @Test
29082 public void testConstructor218() throws WrongSizeException {
29083 final Collection<StandardDeviationBodyKinematics> measurements =
29084 Collections.emptyList();
29085
29086 final Matrix ba = generateBa();
29087 final double[] bias = ba.getBuffer();
29088 final double biasX = ba.getElementAtIndex(0);
29089 final double biasY = ba.getElementAtIndex(1);
29090 final double biasZ = ba.getElementAtIndex(2);
29091
29092 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29093 final double latitude = Math.toRadians(
29094 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29095 final double longitude = Math.toRadians(
29096 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29097 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29098 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29099 final NEDVelocity nedVelocity = new NEDVelocity();
29100 final ECEFPosition ecefPosition = new ECEFPosition();
29101 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29102 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29103 ecefPosition, ecefVelocity);
29104 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29105 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29106 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29107
29108 KnownGravityNormAccelerometerCalibrator calibrator =
29109 new KnownGravityNormAccelerometerCalibrator(
29110 gravityNorm, measurements,
29111 true, bias, this);
29112
29113
29114 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29115 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29116 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29117 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29118 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29119 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29120 final Acceleration bx2 = new Acceleration(0.0,
29121 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29122 calibrator.getInitialBiasXAsAcceleration(bx2);
29123 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29124 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29125 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29126 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29127 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29128 final Acceleration by2 = new Acceleration(0.0,
29129 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29130 calibrator.getInitialBiasYAsAcceleration(by2);
29131 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29132 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29133 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29134 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29135 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29136 final Acceleration bz2 = new Acceleration(0.0,
29137 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29138 calibrator.getInitialBiasZAsAcceleration(bz2);
29139 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29140 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29141 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29142 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29143 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29144 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29145 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29146 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29147 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29148 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29149 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29150 final double[] bias1 = calibrator.getInitialBias();
29151 assertArrayEquals(bias1, bias, 0.0);
29152 final double[] bias2 = new double[3];
29153 calibrator.getInitialBias(bias2);
29154 assertArrayEquals(bias1, bias2, 0.0);
29155 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29156 assertEquals(b1, ba);
29157 final Matrix b2 = new Matrix(3, 1);
29158 calibrator.getInitialBiasAsMatrix(b2);
29159 assertEquals(b1, b2);
29160 final Matrix ma1 = calibrator.getInitialMa();
29161 assertEquals(ma1, new Matrix(3, 3));
29162 final Matrix ma2 = new Matrix(3, 3);
29163 calibrator.getInitialMa(ma2);
29164 assertEquals(ma1, ma2);
29165 assertSame(calibrator.getMeasurements(), measurements);
29166 assertTrue(calibrator.isCommonAxisUsed());
29167 assertSame(calibrator.getListener(), this);
29168 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29169 assertFalse(calibrator.isReady());
29170 assertFalse(calibrator.isRunning());
29171 assertNull(calibrator.getEstimatedBiases());
29172 assertFalse(calibrator.getEstimatedBiases(null));
29173 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29174 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29175 assertNull(calibrator.getEstimatedBiasFx());
29176 assertNull(calibrator.getEstimatedBiasFy());
29177 assertNull(calibrator.getEstimatedBiasFz());
29178 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29179 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29180 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29181 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29182 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29183 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29184 assertNull(calibrator.getEstimatedMa());
29185 assertNull(calibrator.getEstimatedSx());
29186 assertNull(calibrator.getEstimatedSy());
29187 assertNull(calibrator.getEstimatedSz());
29188 assertNull(calibrator.getEstimatedMxy());
29189 assertNull(calibrator.getEstimatedMxz());
29190 assertNull(calibrator.getEstimatedMyx());
29191 assertNull(calibrator.getEstimatedMyz());
29192 assertNull(calibrator.getEstimatedMzx());
29193 assertNull(calibrator.getEstimatedMzy());
29194 assertNull(calibrator.getEstimatedCovariance());
29195 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29196 assertNotNull(calibrator.getGroundTruthGravityNorm());
29197 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29198 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29199 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29200 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29201 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29202 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29203 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29204
29205
29206 final Acceleration invalidGravityNorm = new Acceleration(
29207 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29208
29209 calibrator = null;
29210 try {
29211 calibrator = new KnownGravityNormAccelerometerCalibrator(
29212 invalidGravityNorm, measurements,
29213 true, bias, this);
29214 fail("IllegalArgumentException expected but not thrown");
29215 } catch (final IllegalArgumentException ignore) {
29216 }
29217 try {
29218 calibrator = new KnownGravityNormAccelerometerCalibrator(
29219 gravityNorm, measurements,
29220 true, new double[1], this);
29221 fail("IllegalArgumentException expected but not thrown");
29222 } catch (final IllegalArgumentException ignore) {
29223 }
29224 assertNull(calibrator);
29225 }
29226
29227 @Test
29228 public void testConstructor219() throws WrongSizeException {
29229 final Matrix ba = generateBa();
29230 final double[] bias = ba.getBuffer();
29231 final double biasX = ba.getElementAtIndex(0);
29232 final double biasY = ba.getElementAtIndex(1);
29233 final double biasZ = ba.getElementAtIndex(2);
29234
29235 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29236 final double latitude = Math.toRadians(
29237 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29238 final double longitude = Math.toRadians(
29239 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29240 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29241 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29242 final NEDVelocity nedVelocity = new NEDVelocity();
29243 final ECEFPosition ecefPosition = new ECEFPosition();
29244 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29245 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29246 ecefPosition, ecefVelocity);
29247 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29248 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29249 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29250
29251 KnownGravityNormAccelerometerCalibrator calibrator =
29252 new KnownGravityNormAccelerometerCalibrator(
29253 gravityNorm, ba);
29254
29255
29256 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29257 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29258 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29259 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29260 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29261 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29262 final Acceleration bx2 = new Acceleration(0.0,
29263 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29264 calibrator.getInitialBiasXAsAcceleration(bx2);
29265 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29266 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29267 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29268 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29269 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29270 final Acceleration by2 = new Acceleration(0.0,
29271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29272 calibrator.getInitialBiasYAsAcceleration(by2);
29273 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29274 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29275 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29276 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29277 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29278 final Acceleration bz2 = new Acceleration(0.0,
29279 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29280 calibrator.getInitialBiasZAsAcceleration(bz2);
29281 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29282 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29283 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29284 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29285 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29286 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29287 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29288 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29289 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29290 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29291 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29292 final double[] bias1 = calibrator.getInitialBias();
29293 assertArrayEquals(bias1, bias, 0.0);
29294 final double[] bias2 = new double[3];
29295 calibrator.getInitialBias(bias2);
29296 assertArrayEquals(bias1, bias2, 0.0);
29297 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29298 assertEquals(b1, ba);
29299 final Matrix b2 = new Matrix(3, 1);
29300 calibrator.getInitialBiasAsMatrix(b2);
29301 assertEquals(b1, b2);
29302 final Matrix ma1 = calibrator.getInitialMa();
29303 assertEquals(ma1, new Matrix(3, 3));
29304 final Matrix ma2 = new Matrix(3, 3);
29305 calibrator.getInitialMa(ma2);
29306 assertEquals(ma1, ma2);
29307 assertNull(calibrator.getMeasurements());
29308 assertFalse(calibrator.isCommonAxisUsed());
29309 assertNull(calibrator.getListener());
29310 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29311 assertFalse(calibrator.isReady());
29312 assertFalse(calibrator.isRunning());
29313 assertNull(calibrator.getEstimatedBiases());
29314 assertFalse(calibrator.getEstimatedBiases(null));
29315 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29316 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29317 assertNull(calibrator.getEstimatedBiasFx());
29318 assertNull(calibrator.getEstimatedBiasFy());
29319 assertNull(calibrator.getEstimatedBiasFz());
29320 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29321 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29322 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29323 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29324 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29325 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29326 assertNull(calibrator.getEstimatedMa());
29327 assertNull(calibrator.getEstimatedSx());
29328 assertNull(calibrator.getEstimatedSy());
29329 assertNull(calibrator.getEstimatedSz());
29330 assertNull(calibrator.getEstimatedMxy());
29331 assertNull(calibrator.getEstimatedMxz());
29332 assertNull(calibrator.getEstimatedMyx());
29333 assertNull(calibrator.getEstimatedMyz());
29334 assertNull(calibrator.getEstimatedMzx());
29335 assertNull(calibrator.getEstimatedMzy());
29336 assertNull(calibrator.getEstimatedCovariance());
29337 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29338 assertNotNull(calibrator.getGroundTruthGravityNorm());
29339 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29340 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29341 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29342 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29343 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29344 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29345 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29346
29347
29348 final Acceleration invalidGravityNorm = new Acceleration(
29349 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29350
29351 calibrator = null;
29352 try {
29353 calibrator = new KnownGravityNormAccelerometerCalibrator(
29354 invalidGravityNorm, ba);
29355 fail("IllegalArgumentException expected but not thrown");
29356 } catch (final IllegalArgumentException ignore) {
29357 }
29358 try {
29359 calibrator = new KnownGravityNormAccelerometerCalibrator(
29360 gravityNorm,
29361 new Matrix(1, 1));
29362 fail("IllegalArgumentException expected but not thrown");
29363 } catch (final IllegalArgumentException ignore) {
29364 }
29365 try {
29366 calibrator = new KnownGravityNormAccelerometerCalibrator(
29367 gravityNorm, new Matrix(1, 3));
29368 fail("IllegalArgumentException expected but not thrown");
29369 } catch (final IllegalArgumentException ignore) {
29370 }
29371 assertNull(calibrator);
29372 }
29373
29374 @Test
29375 public void testConstructor220() throws WrongSizeException {
29376 final Matrix ba = generateBa();
29377 final double[] bias = ba.getBuffer();
29378 final double biasX = ba.getElementAtIndex(0);
29379 final double biasY = ba.getElementAtIndex(1);
29380 final double biasZ = ba.getElementAtIndex(2);
29381
29382 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29383 final double latitude = Math.toRadians(
29384 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29385 final double longitude = Math.toRadians(
29386 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29387 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29388 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29389 final NEDVelocity nedVelocity = new NEDVelocity();
29390 final ECEFPosition ecefPosition = new ECEFPosition();
29391 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29392 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29393 ecefPosition, ecefVelocity);
29394 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29395 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29396 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29397
29398 KnownGravityNormAccelerometerCalibrator calibrator =
29399 new KnownGravityNormAccelerometerCalibrator(
29400 gravityNorm, ba, this);
29401
29402
29403 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29404 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29405 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29406 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29407 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29408 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29409 final Acceleration bx2 = new Acceleration(0.0,
29410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29411 calibrator.getInitialBiasXAsAcceleration(bx2);
29412 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29413 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29414 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29415 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29416 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29417 final Acceleration by2 = new Acceleration(0.0,
29418 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29419 calibrator.getInitialBiasYAsAcceleration(by2);
29420 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29421 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29422 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29423 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29424 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29425 final Acceleration bz2 = new Acceleration(0.0,
29426 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29427 calibrator.getInitialBiasZAsAcceleration(bz2);
29428 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29429 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29430 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29431 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29432 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29433 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29434 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29435 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29436 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29437 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29438 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29439 final double[] bias1 = calibrator.getInitialBias();
29440 assertArrayEquals(bias1, bias, 0.0);
29441 final double[] bias2 = new double[3];
29442 calibrator.getInitialBias(bias2);
29443 assertArrayEquals(bias1, bias2, 0.0);
29444 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29445 assertEquals(b1, ba);
29446 final Matrix b2 = new Matrix(3, 1);
29447 calibrator.getInitialBiasAsMatrix(b2);
29448 assertEquals(b1, b2);
29449 final Matrix ma1 = calibrator.getInitialMa();
29450 assertEquals(ma1, new Matrix(3, 3));
29451 final Matrix ma2 = new Matrix(3, 3);
29452 calibrator.getInitialMa(ma2);
29453 assertEquals(ma1, ma2);
29454 assertNull(calibrator.getMeasurements());
29455 assertFalse(calibrator.isCommonAxisUsed());
29456 assertSame(calibrator.getListener(), this);
29457 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29458 assertFalse(calibrator.isReady());
29459 assertFalse(calibrator.isRunning());
29460 assertNull(calibrator.getEstimatedBiases());
29461 assertFalse(calibrator.getEstimatedBiases(null));
29462 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29463 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29464 assertNull(calibrator.getEstimatedBiasFx());
29465 assertNull(calibrator.getEstimatedBiasFy());
29466 assertNull(calibrator.getEstimatedBiasFz());
29467 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29468 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29469 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29470 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29471 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29472 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29473 assertNull(calibrator.getEstimatedMa());
29474 assertNull(calibrator.getEstimatedSx());
29475 assertNull(calibrator.getEstimatedSy());
29476 assertNull(calibrator.getEstimatedSz());
29477 assertNull(calibrator.getEstimatedMxy());
29478 assertNull(calibrator.getEstimatedMxz());
29479 assertNull(calibrator.getEstimatedMyx());
29480 assertNull(calibrator.getEstimatedMyz());
29481 assertNull(calibrator.getEstimatedMzx());
29482 assertNull(calibrator.getEstimatedMzy());
29483 assertNull(calibrator.getEstimatedCovariance());
29484 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29485 assertNotNull(calibrator.getGroundTruthGravityNorm());
29486 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29487 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29488 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29489 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29490 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29491 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29492 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29493
29494
29495 final Acceleration invalidGravityNorm = new Acceleration(
29496 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29497
29498 calibrator = null;
29499 try {
29500 calibrator = new KnownGravityNormAccelerometerCalibrator(
29501 invalidGravityNorm, ba);
29502 fail("IllegalArgumentException expected but not thrown");
29503 } catch (final IllegalArgumentException ignore) {
29504 }
29505 try {
29506 calibrator = new KnownGravityNormAccelerometerCalibrator(
29507 gravityNorm, new Matrix(1, 1),
29508 this);
29509 fail("IllegalArgumentException expected but not thrown");
29510 } catch (final IllegalArgumentException ignore) {
29511 }
29512 try {
29513 calibrator = new KnownGravityNormAccelerometerCalibrator(
29514 gravityNorm, new Matrix(1, 3),
29515 this);
29516 fail("IllegalArgumentException expected but not thrown");
29517 } catch (final IllegalArgumentException ignore) {
29518 }
29519 assertNull(calibrator);
29520 }
29521
29522 @Test
29523 public void testConstructor221() throws WrongSizeException {
29524 final Collection<StandardDeviationBodyKinematics> measurements =
29525 Collections.emptyList();
29526
29527 final Matrix ba = generateBa();
29528 final double[] bias = ba.getBuffer();
29529 final double biasX = ba.getElementAtIndex(0);
29530 final double biasY = ba.getElementAtIndex(1);
29531 final double biasZ = ba.getElementAtIndex(2);
29532
29533 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29534 final double latitude = Math.toRadians(
29535 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29536 final double longitude = Math.toRadians(
29537 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29538 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29539 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29540 final NEDVelocity nedVelocity = new NEDVelocity();
29541 final ECEFPosition ecefPosition = new ECEFPosition();
29542 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29543 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29544 ecefPosition, ecefVelocity);
29545 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29546 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29547 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29548
29549 KnownGravityNormAccelerometerCalibrator calibrator =
29550 new KnownGravityNormAccelerometerCalibrator(
29551 gravityNorm, measurements, ba);
29552
29553
29554 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29555 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29556 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29557 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29558 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29559 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29560 final Acceleration bx2 = new Acceleration(0.0,
29561 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29562 calibrator.getInitialBiasXAsAcceleration(bx2);
29563 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29564 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29565 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29566 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29567 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29568 final Acceleration by2 = new Acceleration(0.0,
29569 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29570 calibrator.getInitialBiasYAsAcceleration(by2);
29571 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29572 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29573 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29574 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29575 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29576 final Acceleration bz2 = new Acceleration(0.0,
29577 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29578 calibrator.getInitialBiasZAsAcceleration(bz2);
29579 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29580 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29581 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29582 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29583 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29584 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29585 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29586 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29587 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29588 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29589 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29590 final double[] bias1 = calibrator.getInitialBias();
29591 assertArrayEquals(bias1, bias, 0.0);
29592 final double[] bias2 = new double[3];
29593 calibrator.getInitialBias(bias2);
29594 assertArrayEquals(bias1, bias2, 0.0);
29595 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29596 assertEquals(b1, ba);
29597 final Matrix b2 = new Matrix(3, 1);
29598 calibrator.getInitialBiasAsMatrix(b2);
29599 assertEquals(b1, b2);
29600 final Matrix ma1 = calibrator.getInitialMa();
29601 assertEquals(ma1, new Matrix(3, 3));
29602 final Matrix ma2 = new Matrix(3, 3);
29603 calibrator.getInitialMa(ma2);
29604 assertEquals(ma1, ma2);
29605 assertSame(calibrator.getMeasurements(), measurements);
29606 assertFalse(calibrator.isCommonAxisUsed());
29607 assertNull(calibrator.getListener());
29608 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29609 assertFalse(calibrator.isReady());
29610 assertFalse(calibrator.isRunning());
29611 assertNull(calibrator.getEstimatedBiases());
29612 assertFalse(calibrator.getEstimatedBiases(null));
29613 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29614 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29615 assertNull(calibrator.getEstimatedBiasFx());
29616 assertNull(calibrator.getEstimatedBiasFy());
29617 assertNull(calibrator.getEstimatedBiasFz());
29618 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29619 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29620 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29621 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29622 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29623 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29624 assertNull(calibrator.getEstimatedMa());
29625 assertNull(calibrator.getEstimatedSx());
29626 assertNull(calibrator.getEstimatedSy());
29627 assertNull(calibrator.getEstimatedSz());
29628 assertNull(calibrator.getEstimatedMxy());
29629 assertNull(calibrator.getEstimatedMxz());
29630 assertNull(calibrator.getEstimatedMyx());
29631 assertNull(calibrator.getEstimatedMyz());
29632 assertNull(calibrator.getEstimatedMzx());
29633 assertNull(calibrator.getEstimatedMzy());
29634 assertNull(calibrator.getEstimatedCovariance());
29635 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29636 assertNotNull(calibrator.getGroundTruthGravityNorm());
29637 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29638 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29639 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29640 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29641 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29642 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29643 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29644
29645
29646 final Acceleration invalidGravityNorm = new Acceleration(
29647 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29648
29649 calibrator = null;
29650 try {
29651 calibrator = new KnownGravityNormAccelerometerCalibrator(
29652 invalidGravityNorm, measurements, ba);
29653 fail("IllegalArgumentException expected but not thrown");
29654 } catch (final IllegalArgumentException ignore) {
29655 }
29656 try {
29657 calibrator = new KnownGravityNormAccelerometerCalibrator(
29658 gravityNorm, measurements,
29659 new Matrix(1, 1));
29660 fail("IllegalArgumentException expected but not thrown");
29661 } catch (final IllegalArgumentException ignore) {
29662 }
29663 try {
29664 calibrator = new KnownGravityNormAccelerometerCalibrator(
29665 gravityNorm, measurements,
29666 new Matrix(1, 3));
29667 fail("IllegalArgumentException expected but not thrown");
29668 } catch (final IllegalArgumentException ignore) {
29669 }
29670 assertNull(calibrator);
29671 }
29672
29673 @Test
29674 public void testConstructor222() throws WrongSizeException {
29675 final Collection<StandardDeviationBodyKinematics> measurements =
29676 Collections.emptyList();
29677
29678 final Matrix ba = generateBa();
29679 final double[] bias = ba.getBuffer();
29680 final double biasX = ba.getElementAtIndex(0);
29681 final double biasY = ba.getElementAtIndex(1);
29682 final double biasZ = ba.getElementAtIndex(2);
29683
29684 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29685 final double latitude = Math.toRadians(
29686 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29687 final double longitude = Math.toRadians(
29688 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29689 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29690 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29691 final NEDVelocity nedVelocity = new NEDVelocity();
29692 final ECEFPosition ecefPosition = new ECEFPosition();
29693 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29694 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29695 ecefPosition, ecefVelocity);
29696 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29697 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29698 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29699
29700 KnownGravityNormAccelerometerCalibrator calibrator =
29701 new KnownGravityNormAccelerometerCalibrator(
29702 gravityNorm, measurements,
29703 ba, this);
29704
29705
29706 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29707 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29708 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29709 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29710 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29711 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29712 final Acceleration bx2 = new Acceleration(0.0,
29713 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29714 calibrator.getInitialBiasXAsAcceleration(bx2);
29715 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29716 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29717 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29718 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29719 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29720 final Acceleration by2 = new Acceleration(0.0,
29721 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29722 calibrator.getInitialBiasYAsAcceleration(by2);
29723 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29724 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29725 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29726 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29727 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29728 final Acceleration bz2 = new Acceleration(0.0,
29729 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29730 calibrator.getInitialBiasZAsAcceleration(bz2);
29731 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29732 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29733 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29734 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29735 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29736 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29737 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29738 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29739 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29740 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29741 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29742 final double[] bias1 = calibrator.getInitialBias();
29743 assertArrayEquals(bias1, bias, 0.0);
29744 final double[] bias2 = new double[3];
29745 calibrator.getInitialBias(bias2);
29746 assertArrayEquals(bias1, bias2, 0.0);
29747 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29748 assertEquals(b1, ba);
29749 final Matrix b2 = new Matrix(3, 1);
29750 calibrator.getInitialBiasAsMatrix(b2);
29751 assertEquals(b1, b2);
29752 final Matrix ma1 = calibrator.getInitialMa();
29753 assertEquals(ma1, new Matrix(3, 3));
29754 final Matrix ma2 = new Matrix(3, 3);
29755 calibrator.getInitialMa(ma2);
29756 assertEquals(ma1, ma2);
29757 assertSame(calibrator.getMeasurements(), measurements);
29758 assertFalse(calibrator.isCommonAxisUsed());
29759 assertSame(calibrator.getListener(), this);
29760 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29761 assertFalse(calibrator.isReady());
29762 assertFalse(calibrator.isRunning());
29763 assertNull(calibrator.getEstimatedBiases());
29764 assertFalse(calibrator.getEstimatedBiases(null));
29765 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29766 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29767 assertNull(calibrator.getEstimatedBiasFx());
29768 assertNull(calibrator.getEstimatedBiasFy());
29769 assertNull(calibrator.getEstimatedBiasFz());
29770 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29771 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29772 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29773 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29774 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29775 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29776 assertNull(calibrator.getEstimatedMa());
29777 assertNull(calibrator.getEstimatedSx());
29778 assertNull(calibrator.getEstimatedSy());
29779 assertNull(calibrator.getEstimatedSz());
29780 assertNull(calibrator.getEstimatedMxy());
29781 assertNull(calibrator.getEstimatedMxz());
29782 assertNull(calibrator.getEstimatedMyx());
29783 assertNull(calibrator.getEstimatedMyz());
29784 assertNull(calibrator.getEstimatedMzx());
29785 assertNull(calibrator.getEstimatedMzy());
29786 assertNull(calibrator.getEstimatedCovariance());
29787 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29788 assertNotNull(calibrator.getGroundTruthGravityNorm());
29789 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29790 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29791 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29792 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29793 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29794 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29795 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29796
29797
29798 final Acceleration invalidGravityNorm = new Acceleration(
29799 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29800
29801 calibrator = null;
29802 try {
29803 calibrator = new KnownGravityNormAccelerometerCalibrator(
29804 invalidGravityNorm, measurements,
29805 ba, this);
29806 fail("IllegalArgumentException expected but not thrown");
29807 } catch (final IllegalArgumentException ignore) {
29808 }
29809 try {
29810 calibrator = new KnownGravityNormAccelerometerCalibrator(
29811 gravityNorm, measurements,
29812 new Matrix(1, 1), this);
29813 fail("IllegalArgumentException expected but not thrown");
29814 } catch (final IllegalArgumentException ignore) {
29815 }
29816 try {
29817 calibrator = new KnownGravityNormAccelerometerCalibrator(
29818 gravityNorm, measurements,
29819 new Matrix(1, 3), this);
29820 fail("IllegalArgumentException expected but not thrown");
29821 } catch (final IllegalArgumentException ignore) {
29822 }
29823 assertNull(calibrator);
29824 }
29825
29826 @Test
29827 public void testConstructor223() throws WrongSizeException {
29828 final Matrix ba = generateBa();
29829 final double[] bias = ba.getBuffer();
29830 final double biasX = ba.getElementAtIndex(0);
29831 final double biasY = ba.getElementAtIndex(1);
29832 final double biasZ = ba.getElementAtIndex(2);
29833
29834 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29835 final double latitude = Math.toRadians(
29836 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29837 final double longitude = Math.toRadians(
29838 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29839 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29840 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29841 final NEDVelocity nedVelocity = new NEDVelocity();
29842 final ECEFPosition ecefPosition = new ECEFPosition();
29843 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29844 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29845 ecefPosition, ecefVelocity);
29846 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29847 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29848 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29849
29850 KnownGravityNormAccelerometerCalibrator calibrator =
29851 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
29852 true, ba);
29853
29854
29855 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29856 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29857 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29858 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29859 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29860 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29861 final Acceleration bx2 = new Acceleration(0.0,
29862 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29863 calibrator.getInitialBiasXAsAcceleration(bx2);
29864 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29865 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29866 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29867 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29868 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29869 final Acceleration by2 = new Acceleration(0.0,
29870 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29871 calibrator.getInitialBiasYAsAcceleration(by2);
29872 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29873 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29874 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29875 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29876 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29877 final Acceleration bz2 = new Acceleration(0.0,
29878 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29879 calibrator.getInitialBiasZAsAcceleration(bz2);
29880 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29881 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29882 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29883 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29884 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29885 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29886 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29887 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29888 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29889 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29890 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29891 final double[] bias1 = calibrator.getInitialBias();
29892 assertArrayEquals(bias1, bias, 0.0);
29893 final double[] bias2 = new double[3];
29894 calibrator.getInitialBias(bias2);
29895 assertArrayEquals(bias1, bias2, 0.0);
29896 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29897 assertEquals(b1, ba);
29898 final Matrix b2 = new Matrix(3, 1);
29899 calibrator.getInitialBiasAsMatrix(b2);
29900 assertEquals(b1, b2);
29901 final Matrix ma1 = calibrator.getInitialMa();
29902 assertEquals(ma1, new Matrix(3, 3));
29903 final Matrix ma2 = new Matrix(3, 3);
29904 calibrator.getInitialMa(ma2);
29905 assertEquals(ma1, ma2);
29906 assertNull(calibrator.getMeasurements());
29907 assertTrue(calibrator.isCommonAxisUsed());
29908 assertNull(calibrator.getListener());
29909 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29910 assertFalse(calibrator.isReady());
29911 assertFalse(calibrator.isRunning());
29912 assertNull(calibrator.getEstimatedBiases());
29913 assertFalse(calibrator.getEstimatedBiases(null));
29914 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29915 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29916 assertNull(calibrator.getEstimatedBiasFx());
29917 assertNull(calibrator.getEstimatedBiasFy());
29918 assertNull(calibrator.getEstimatedBiasFz());
29919 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29920 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29921 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29922 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29923 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29924 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29925 assertNull(calibrator.getEstimatedMa());
29926 assertNull(calibrator.getEstimatedSx());
29927 assertNull(calibrator.getEstimatedSy());
29928 assertNull(calibrator.getEstimatedSz());
29929 assertNull(calibrator.getEstimatedMxy());
29930 assertNull(calibrator.getEstimatedMxz());
29931 assertNull(calibrator.getEstimatedMyx());
29932 assertNull(calibrator.getEstimatedMyz());
29933 assertNull(calibrator.getEstimatedMzx());
29934 assertNull(calibrator.getEstimatedMzy());
29935 assertNull(calibrator.getEstimatedCovariance());
29936 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29937 assertNotNull(calibrator.getGroundTruthGravityNorm());
29938 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29939 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29940 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29941 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29942 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29943 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29944 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29945
29946
29947 final Acceleration invalidGravityNorm = new Acceleration(
29948 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29949
29950 calibrator = null;
29951 try {
29952 calibrator = new KnownGravityNormAccelerometerCalibrator(
29953 invalidGravityNorm, true, ba);
29954 fail("IllegalArgumentException expected but not thrown");
29955 } catch (final IllegalArgumentException ignore) {
29956 }
29957 try {
29958 calibrator = new KnownGravityNormAccelerometerCalibrator(
29959 gravityNorm, true,
29960 new Matrix(1, 1));
29961 fail("IllegalArgumentException expected but not thrown");
29962 } catch (final IllegalArgumentException ignore) {
29963 }
29964 try {
29965 calibrator = new KnownGravityNormAccelerometerCalibrator(
29966 gravityNorm, true,
29967 new Matrix(1, 3));
29968 fail("IllegalArgumentException expected but not thrown");
29969 } catch (final IllegalArgumentException ignore) {
29970 }
29971 assertNull(calibrator);
29972 }
29973
29974 @Test
29975 public void testConstructor224() throws WrongSizeException {
29976 final Matrix ba = generateBa();
29977 final double[] bias = ba.getBuffer();
29978 final double biasX = ba.getElementAtIndex(0);
29979 final double biasY = ba.getElementAtIndex(1);
29980 final double biasZ = ba.getElementAtIndex(2);
29981
29982 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29983 final double latitude = Math.toRadians(
29984 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29985 final double longitude = Math.toRadians(
29986 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29987 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29988 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29989 final NEDVelocity nedVelocity = new NEDVelocity();
29990 final ECEFPosition ecefPosition = new ECEFPosition();
29991 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29992 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29993 ecefPosition, ecefVelocity);
29994 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29995 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29996 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29997
29998 KnownGravityNormAccelerometerCalibrator calibrator =
29999 new KnownGravityNormAccelerometerCalibrator(
30000 gravityNorm, true, ba, this);
30001
30002
30003 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30004 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30005 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30006 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30007 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30008 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30009 final Acceleration bx2 = new Acceleration(0.0,
30010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30011 calibrator.getInitialBiasXAsAcceleration(bx2);
30012 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30013 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30014 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30015 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30016 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30017 final Acceleration by2 = new Acceleration(0.0,
30018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30019 calibrator.getInitialBiasYAsAcceleration(by2);
30020 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30021 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30022 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30023 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30024 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30025 final Acceleration bz2 = new Acceleration(0.0,
30026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30027 calibrator.getInitialBiasZAsAcceleration(bz2);
30028 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30029 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30030 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30031 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30032 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30033 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30034 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30035 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30036 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30037 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30038 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30039 final double[] bias1 = calibrator.getInitialBias();
30040 assertArrayEquals(bias1, bias, 0.0);
30041 final double[] bias2 = new double[3];
30042 calibrator.getInitialBias(bias2);
30043 assertArrayEquals(bias1, bias2, 0.0);
30044 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30045 assertEquals(b1, ba);
30046 final Matrix b2 = new Matrix(3, 1);
30047 calibrator.getInitialBiasAsMatrix(b2);
30048 assertEquals(b1, b2);
30049 final Matrix ma1 = calibrator.getInitialMa();
30050 assertEquals(ma1, new Matrix(3, 3));
30051 final Matrix ma2 = new Matrix(3, 3);
30052 calibrator.getInitialMa(ma2);
30053 assertEquals(ma1, ma2);
30054 assertNull(calibrator.getMeasurements());
30055 assertTrue(calibrator.isCommonAxisUsed());
30056 assertSame(calibrator.getListener(), this);
30057 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30058 assertFalse(calibrator.isReady());
30059 assertFalse(calibrator.isRunning());
30060 assertNull(calibrator.getEstimatedBiases());
30061 assertFalse(calibrator.getEstimatedBiases(null));
30062 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30063 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30064 assertNull(calibrator.getEstimatedBiasFx());
30065 assertNull(calibrator.getEstimatedBiasFy());
30066 assertNull(calibrator.getEstimatedBiasFz());
30067 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30068 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30069 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30070 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30071 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30072 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30073 assertNull(calibrator.getEstimatedMa());
30074 assertNull(calibrator.getEstimatedSx());
30075 assertNull(calibrator.getEstimatedSy());
30076 assertNull(calibrator.getEstimatedSz());
30077 assertNull(calibrator.getEstimatedMxy());
30078 assertNull(calibrator.getEstimatedMxz());
30079 assertNull(calibrator.getEstimatedMyx());
30080 assertNull(calibrator.getEstimatedMyz());
30081 assertNull(calibrator.getEstimatedMzx());
30082 assertNull(calibrator.getEstimatedMzy());
30083 assertNull(calibrator.getEstimatedCovariance());
30084 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30085 assertNotNull(calibrator.getGroundTruthGravityNorm());
30086 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30087 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30088 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30089 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30090 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30091 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30092 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30093
30094
30095 final Acceleration invalidGravityNorm = new Acceleration(
30096 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30097
30098 calibrator = null;
30099 try {
30100 calibrator = new KnownGravityNormAccelerometerCalibrator(
30101 invalidGravityNorm, true, ba, this);
30102 fail("IllegalArgumentException expected but not thrown");
30103 } catch (final IllegalArgumentException ignore) {
30104 }
30105 try {
30106 calibrator = new KnownGravityNormAccelerometerCalibrator(
30107 gravityNorm, true, new Matrix(1, 1),
30108 this);
30109 fail("IllegalArgumentException expected but not thrown");
30110 } catch (final IllegalArgumentException ignore) {
30111 }
30112 try {
30113 calibrator = new KnownGravityNormAccelerometerCalibrator(
30114 gravityNorm, true, new Matrix(1, 3),
30115 this);
30116 fail("IllegalArgumentException expected but not thrown");
30117 } catch (final IllegalArgumentException ignore) {
30118 }
30119 assertNull(calibrator);
30120 }
30121
30122 @Test
30123 public void testConstructor225() throws WrongSizeException {
30124 final Collection<StandardDeviationBodyKinematics> measurements =
30125 Collections.emptyList();
30126
30127 final Matrix ba = generateBa();
30128 final double[] bias = ba.getBuffer();
30129 final double biasX = ba.getElementAtIndex(0);
30130 final double biasY = ba.getElementAtIndex(1);
30131 final double biasZ = ba.getElementAtIndex(2);
30132
30133 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30134 final double latitude = Math.toRadians(
30135 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30136 final double longitude = Math.toRadians(
30137 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30138 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30139 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30140 final NEDVelocity nedVelocity = new NEDVelocity();
30141 final ECEFPosition ecefPosition = new ECEFPosition();
30142 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30143 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30144 ecefPosition, ecefVelocity);
30145 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30146 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30147 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30148
30149 KnownGravityNormAccelerometerCalibrator calibrator =
30150 new KnownGravityNormAccelerometerCalibrator(
30151 gravityNorm, measurements,
30152 true, ba);
30153
30154
30155 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30156 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30157 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30158 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30159 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30160 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30161 final Acceleration bx2 = new Acceleration(0.0,
30162 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30163 calibrator.getInitialBiasXAsAcceleration(bx2);
30164 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30165 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30166 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30167 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30168 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30169 final Acceleration by2 = new Acceleration(0.0,
30170 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30171 calibrator.getInitialBiasYAsAcceleration(by2);
30172 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30173 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30174 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30175 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30176 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30177 final Acceleration bz2 = new Acceleration(0.0,
30178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30179 calibrator.getInitialBiasZAsAcceleration(bz2);
30180 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30181 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30182 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30183 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30184 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30185 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30186 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30187 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30188 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30189 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30190 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30191 final double[] bias1 = calibrator.getInitialBias();
30192 assertArrayEquals(bias1, bias, 0.0);
30193 final double[] bias2 = new double[3];
30194 calibrator.getInitialBias(bias2);
30195 assertArrayEquals(bias1, bias2, 0.0);
30196 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30197 assertEquals(b1, ba);
30198 final Matrix b2 = new Matrix(3, 1);
30199 calibrator.getInitialBiasAsMatrix(b2);
30200 assertEquals(b1, b2);
30201 final Matrix ma1 = calibrator.getInitialMa();
30202 assertEquals(ma1, new Matrix(3, 3));
30203 final Matrix ma2 = new Matrix(3, 3);
30204 calibrator.getInitialMa(ma2);
30205 assertEquals(ma1, ma2);
30206 assertSame(calibrator.getMeasurements(), measurements);
30207 assertTrue(calibrator.isCommonAxisUsed());
30208 assertNull(calibrator.getListener());
30209 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30210 assertFalse(calibrator.isReady());
30211 assertFalse(calibrator.isRunning());
30212 assertNull(calibrator.getEstimatedBiases());
30213 assertFalse(calibrator.getEstimatedBiases(null));
30214 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30215 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30216 assertNull(calibrator.getEstimatedBiasFx());
30217 assertNull(calibrator.getEstimatedBiasFy());
30218 assertNull(calibrator.getEstimatedBiasFz());
30219 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30220 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30221 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30222 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30223 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30224 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30225 assertNull(calibrator.getEstimatedMa());
30226 assertNull(calibrator.getEstimatedSx());
30227 assertNull(calibrator.getEstimatedSy());
30228 assertNull(calibrator.getEstimatedSz());
30229 assertNull(calibrator.getEstimatedMxy());
30230 assertNull(calibrator.getEstimatedMxz());
30231 assertNull(calibrator.getEstimatedMyx());
30232 assertNull(calibrator.getEstimatedMyz());
30233 assertNull(calibrator.getEstimatedMzx());
30234 assertNull(calibrator.getEstimatedMzy());
30235 assertNull(calibrator.getEstimatedCovariance());
30236 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30237 assertNotNull(calibrator.getGroundTruthGravityNorm());
30238 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30239 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30240 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30241 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30242 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30243 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30244 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30245
30246
30247 final Acceleration invalidGravityNorm = new Acceleration(
30248 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30249
30250 calibrator = null;
30251 try {
30252 calibrator = new KnownGravityNormAccelerometerCalibrator(
30253 invalidGravityNorm, measurements,
30254 true, ba);
30255 fail("IllegalArgumentException expected but not thrown");
30256 } catch (final IllegalArgumentException ignore) {
30257 }
30258 try {
30259 calibrator = new KnownGravityNormAccelerometerCalibrator(
30260 gravityNorm, measurements, true,
30261 new Matrix(1, 1));
30262 fail("IllegalArgumentException expected but not thrown");
30263 } catch (final IllegalArgumentException ignore) {
30264 }
30265 try {
30266 calibrator = new KnownGravityNormAccelerometerCalibrator(
30267 gravityNorm, measurements, true,
30268 new Matrix(1, 3));
30269 fail("IllegalArgumentException expected but not thrown");
30270 } catch (final IllegalArgumentException ignore) {
30271 }
30272 assertNull(calibrator);
30273 }
30274
30275 @Test
30276 public void testConstructor226() throws WrongSizeException {
30277 final Collection<StandardDeviationBodyKinematics> measurements =
30278 Collections.emptyList();
30279
30280 final Matrix ba = generateBa();
30281 final double[] bias = ba.getBuffer();
30282 final double biasX = ba.getElementAtIndex(0);
30283 final double biasY = ba.getElementAtIndex(1);
30284 final double biasZ = ba.getElementAtIndex(2);
30285
30286 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30287 final double latitude = Math.toRadians(
30288 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30289 final double longitude = Math.toRadians(
30290 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30291 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30292 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30293 final NEDVelocity nedVelocity = new NEDVelocity();
30294 final ECEFPosition ecefPosition = new ECEFPosition();
30295 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30296 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30297 ecefPosition, ecefVelocity);
30298 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30299 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30300 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30301
30302 KnownGravityNormAccelerometerCalibrator calibrator =
30303 new KnownGravityNormAccelerometerCalibrator(
30304 gravityNorm, measurements,
30305 true, ba, this);
30306
30307
30308 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30309 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30310 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30311 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30312 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30313 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30314 final Acceleration bx2 = new Acceleration(0.0,
30315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30316 calibrator.getInitialBiasXAsAcceleration(bx2);
30317 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30318 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30319 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30320 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30321 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30322 final Acceleration by2 = new Acceleration(0.0,
30323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30324 calibrator.getInitialBiasYAsAcceleration(by2);
30325 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30326 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30327 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30328 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30329 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30330 final Acceleration bz2 = new Acceleration(0.0,
30331 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30332 calibrator.getInitialBiasZAsAcceleration(bz2);
30333 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30334 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30335 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30336 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30337 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30338 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30339 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30340 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30341 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30342 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30343 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30344 final double[] bias1 = calibrator.getInitialBias();
30345 assertArrayEquals(bias1, bias, 0.0);
30346 final double[] bias2 = new double[3];
30347 calibrator.getInitialBias(bias2);
30348 assertArrayEquals(bias1, bias2, 0.0);
30349 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30350 assertEquals(b1, ba);
30351 final Matrix b2 = new Matrix(3, 1);
30352 calibrator.getInitialBiasAsMatrix(b2);
30353 assertEquals(b1, b2);
30354 final Matrix ma1 = calibrator.getInitialMa();
30355 assertEquals(ma1, new Matrix(3, 3));
30356 final Matrix ma2 = new Matrix(3, 3);
30357 calibrator.getInitialMa(ma2);
30358 assertEquals(ma1, ma2);
30359 assertSame(calibrator.getMeasurements(), measurements);
30360 assertTrue(calibrator.isCommonAxisUsed());
30361 assertSame(calibrator.getListener(), this);
30362 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30363 assertFalse(calibrator.isReady());
30364 assertFalse(calibrator.isRunning());
30365 assertNull(calibrator.getEstimatedBiases());
30366 assertFalse(calibrator.getEstimatedBiases(null));
30367 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30368 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30369 assertNull(calibrator.getEstimatedBiasFx());
30370 assertNull(calibrator.getEstimatedBiasFy());
30371 assertNull(calibrator.getEstimatedBiasFz());
30372 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30373 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30374 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30375 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30376 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30377 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30378 assertNull(calibrator.getEstimatedMa());
30379 assertNull(calibrator.getEstimatedSx());
30380 assertNull(calibrator.getEstimatedSy());
30381 assertNull(calibrator.getEstimatedSz());
30382 assertNull(calibrator.getEstimatedMxy());
30383 assertNull(calibrator.getEstimatedMxz());
30384 assertNull(calibrator.getEstimatedMyx());
30385 assertNull(calibrator.getEstimatedMyz());
30386 assertNull(calibrator.getEstimatedMzx());
30387 assertNull(calibrator.getEstimatedMzy());
30388 assertNull(calibrator.getEstimatedCovariance());
30389 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30390 assertNotNull(calibrator.getGroundTruthGravityNorm());
30391 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30392 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30393 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30394 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30395 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30396 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30397 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30398
30399
30400 final Acceleration invalidGravityNorm = new Acceleration(
30401 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30402
30403 calibrator = null;
30404 try {
30405 calibrator = new KnownGravityNormAccelerometerCalibrator(
30406 invalidGravityNorm, measurements,
30407 true, ba, this);
30408 fail("IllegalArgumentException expected but not thrown");
30409 } catch (final IllegalArgumentException ignore) {
30410 }
30411 try {
30412 calibrator = new KnownGravityNormAccelerometerCalibrator(
30413 gravityNorm, measurements, true,
30414 new Matrix(1, 1), this);
30415 fail("IllegalArgumentException expected but not thrown");
30416 } catch (final IllegalArgumentException ignore) {
30417 }
30418 try {
30419 calibrator = new KnownGravityNormAccelerometerCalibrator(
30420 gravityNorm, measurements, true,
30421 new Matrix(1, 3), this);
30422 fail("IllegalArgumentException expected but not thrown");
30423 } catch (final IllegalArgumentException ignore) {
30424 }
30425 assertNull(calibrator);
30426 }
30427
30428 @Test
30429 public void testConstructor227() throws WrongSizeException {
30430 final Matrix ba = generateBa();
30431 final double[] bias = ba.getBuffer();
30432 final double biasX = ba.getElementAtIndex(0);
30433 final double biasY = ba.getElementAtIndex(1);
30434 final double biasZ = ba.getElementAtIndex(2);
30435
30436 final Matrix ma = generateMaCommonAxis();
30437 final double sx = ma.getElementAt(0, 0);
30438 final double sy = ma.getElementAt(1, 1);
30439 final double sz = ma.getElementAt(2, 2);
30440 final double mxy = ma.getElementAt(0, 1);
30441 final double mxz = ma.getElementAt(0, 2);
30442 final double myx = ma.getElementAt(1, 0);
30443 final double myz = ma.getElementAt(1, 2);
30444 final double mzx = ma.getElementAt(2, 0);
30445 final double mzy = ma.getElementAt(2, 1);
30446
30447 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30448 final double latitude = Math.toRadians(
30449 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30450 final double longitude = Math.toRadians(
30451 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30452 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30453 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30454 final NEDVelocity nedVelocity = new NEDVelocity();
30455 final ECEFPosition ecefPosition = new ECEFPosition();
30456 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30457 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30458 ecefPosition, ecefVelocity);
30459 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30460 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30461 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30462
30463 KnownGravityNormAccelerometerCalibrator calibrator =
30464 new KnownGravityNormAccelerometerCalibrator(
30465 gravityNorm, ba, ma);
30466
30467
30468 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30469 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30470 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30471 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30472 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30473 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30474 final Acceleration bx2 = new Acceleration(0.0,
30475 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30476 calibrator.getInitialBiasXAsAcceleration(bx2);
30477 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30478 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30479 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30480 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30481 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30482 final Acceleration by2 = new Acceleration(0.0,
30483 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30484 calibrator.getInitialBiasYAsAcceleration(by2);
30485 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30486 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30487 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30488 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30489 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30490 final Acceleration bz2 = new Acceleration(0.0,
30491 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30492 calibrator.getInitialBiasZAsAcceleration(bz2);
30493 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30494 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30495 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30496 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30497 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30498 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30499 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30500 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30501 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30502 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30503 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30504 final double[] bias1 = calibrator.getInitialBias();
30505 assertArrayEquals(bias1, bias, 0.0);
30506 final double[] bias2 = new double[3];
30507 calibrator.getInitialBias(bias2);
30508 assertArrayEquals(bias1, bias2, 0.0);
30509 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30510 assertEquals(b1, ba);
30511 final Matrix b2 = new Matrix(3, 1);
30512 calibrator.getInitialBiasAsMatrix(b2);
30513 assertEquals(b1, b2);
30514 final Matrix ma1 = new Matrix(3, 3);
30515 ma1.setSubmatrix(0, 0,
30516 2, 2,
30517 new double[]{sx, myx, mzx,
30518 mxy, sy, mzy,
30519 mxz, myz, sz});
30520 assertEquals(calibrator.getInitialMa(), ma1);
30521 final Matrix ma2 = new Matrix(3, 3);
30522 calibrator.getInitialMa(ma2);
30523 assertEquals(ma1, ma2);
30524 assertNull(calibrator.getMeasurements());
30525 assertFalse(calibrator.isCommonAxisUsed());
30526 assertNull(calibrator.getListener());
30527 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30528 assertFalse(calibrator.isReady());
30529 assertFalse(calibrator.isRunning());
30530 assertNull(calibrator.getEstimatedBiases());
30531 assertFalse(calibrator.getEstimatedBiases(null));
30532 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30533 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30534 assertNull(calibrator.getEstimatedBiasFx());
30535 assertNull(calibrator.getEstimatedBiasFy());
30536 assertNull(calibrator.getEstimatedBiasFz());
30537 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30538 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30539 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30540 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30541 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30542 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30543 assertNull(calibrator.getEstimatedMa());
30544 assertNull(calibrator.getEstimatedSx());
30545 assertNull(calibrator.getEstimatedSy());
30546 assertNull(calibrator.getEstimatedSz());
30547 assertNull(calibrator.getEstimatedMxy());
30548 assertNull(calibrator.getEstimatedMxz());
30549 assertNull(calibrator.getEstimatedMyx());
30550 assertNull(calibrator.getEstimatedMyz());
30551 assertNull(calibrator.getEstimatedMzx());
30552 assertNull(calibrator.getEstimatedMzy());
30553 assertNull(calibrator.getEstimatedCovariance());
30554 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30555 assertNotNull(calibrator.getGroundTruthGravityNorm());
30556 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30557 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30558 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30559 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30560 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30561 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30562 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30563
30564
30565 final Acceleration invalidGravityNorm = new Acceleration(
30566 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30567
30568 calibrator = null;
30569 try {
30570 calibrator = new KnownGravityNormAccelerometerCalibrator(
30571 invalidGravityNorm, ba, ma);
30572 fail("IllegalArgumentException expected but not thrown");
30573 } catch (final IllegalArgumentException ignore) {
30574 }
30575 try {
30576 calibrator = new KnownGravityNormAccelerometerCalibrator(
30577 gravityNorm, new Matrix(1, 1), ma);
30578 fail("IllegalArgumentException expected but not thrown");
30579 } catch (final IllegalArgumentException ignore) {
30580 }
30581 try {
30582 calibrator = new KnownGravityNormAccelerometerCalibrator(
30583 gravityNorm, new Matrix(1, 3), ma);
30584 fail("IllegalArgumentException expected but not thrown");
30585 } catch (final IllegalArgumentException ignore) {
30586 }
30587 try {
30588 calibrator = new KnownGravityNormAccelerometerCalibrator(
30589 gravityNorm, ba, new Matrix(1, 3));
30590 fail("IllegalArgumentException expected but not thrown");
30591 } catch (final IllegalArgumentException ignore) {
30592 }
30593 try {
30594 calibrator = new KnownGravityNormAccelerometerCalibrator(
30595 gravityNorm,
30596 ba, new Matrix(3, 1));
30597 fail("IllegalArgumentException expected but not thrown");
30598 } catch (final IllegalArgumentException ignore) {
30599 }
30600 assertNull(calibrator);
30601 }
30602
30603 @Test
30604 public void testConstructor228() throws WrongSizeException {
30605 final Matrix ba = generateBa();
30606 final double[] bias = ba.getBuffer();
30607 final double biasX = ba.getElementAtIndex(0);
30608 final double biasY = ba.getElementAtIndex(1);
30609 final double biasZ = ba.getElementAtIndex(2);
30610
30611 final Matrix ma = generateMaCommonAxis();
30612 final double sx = ma.getElementAt(0, 0);
30613 final double sy = ma.getElementAt(1, 1);
30614 final double sz = ma.getElementAt(2, 2);
30615 final double mxy = ma.getElementAt(0, 1);
30616 final double mxz = ma.getElementAt(0, 2);
30617 final double myx = ma.getElementAt(1, 0);
30618 final double myz = ma.getElementAt(1, 2);
30619 final double mzx = ma.getElementAt(2, 0);
30620 final double mzy = ma.getElementAt(2, 1);
30621
30622 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30623 final double latitude = Math.toRadians(
30624 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30625 final double longitude = Math.toRadians(
30626 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30627 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30628 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30629 final NEDVelocity nedVelocity = new NEDVelocity();
30630 final ECEFPosition ecefPosition = new ECEFPosition();
30631 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30632 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30633 ecefPosition, ecefVelocity);
30634 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30635 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30636 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30637
30638 KnownGravityNormAccelerometerCalibrator calibrator =
30639 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
30640 ba, ma, this);
30641
30642
30643 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30644 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30645 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30646 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30647 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30648 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30649 final Acceleration bx2 = new Acceleration(0.0,
30650 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30651 calibrator.getInitialBiasXAsAcceleration(bx2);
30652 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30653 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30654 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30655 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30656 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30657 final Acceleration by2 = new Acceleration(0.0,
30658 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30659 calibrator.getInitialBiasYAsAcceleration(by2);
30660 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30661 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30662 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30663 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30664 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30665 final Acceleration bz2 = new Acceleration(0.0,
30666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30667 calibrator.getInitialBiasZAsAcceleration(bz2);
30668 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30669 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30670 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30671 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30672 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30673 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30674 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30675 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30676 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30677 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30678 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30679 final double[] bias1 = calibrator.getInitialBias();
30680 assertArrayEquals(bias1, bias, 0.0);
30681 final double[] bias2 = new double[3];
30682 calibrator.getInitialBias(bias2);
30683 assertArrayEquals(bias1, bias2, 0.0);
30684 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30685 assertEquals(b1, ba);
30686 final Matrix b2 = new Matrix(3, 1);
30687 calibrator.getInitialBiasAsMatrix(b2);
30688 assertEquals(b1, b2);
30689 final Matrix ma1 = new Matrix(3, 3);
30690 ma1.setSubmatrix(0, 0,
30691 2, 2,
30692 new double[]{sx, myx, mzx,
30693 mxy, sy, mzy,
30694 mxz, myz, sz});
30695 assertEquals(calibrator.getInitialMa(), ma1);
30696 final Matrix ma2 = new Matrix(3, 3);
30697 calibrator.getInitialMa(ma2);
30698 assertEquals(ma1, ma2);
30699 assertNull(calibrator.getMeasurements());
30700 assertFalse(calibrator.isCommonAxisUsed());
30701 assertSame(calibrator.getListener(), this);
30702 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30703 assertFalse(calibrator.isReady());
30704 assertFalse(calibrator.isRunning());
30705 assertNull(calibrator.getEstimatedBiases());
30706 assertFalse(calibrator.getEstimatedBiases(null));
30707 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30708 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30709 assertNull(calibrator.getEstimatedBiasFx());
30710 assertNull(calibrator.getEstimatedBiasFy());
30711 assertNull(calibrator.getEstimatedBiasFz());
30712 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30713 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30714 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30715 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30716 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30717 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30718 assertNull(calibrator.getEstimatedMa());
30719 assertNull(calibrator.getEstimatedSx());
30720 assertNull(calibrator.getEstimatedSy());
30721 assertNull(calibrator.getEstimatedSz());
30722 assertNull(calibrator.getEstimatedMxy());
30723 assertNull(calibrator.getEstimatedMxz());
30724 assertNull(calibrator.getEstimatedMyx());
30725 assertNull(calibrator.getEstimatedMyz());
30726 assertNull(calibrator.getEstimatedMzx());
30727 assertNull(calibrator.getEstimatedMzy());
30728 assertNull(calibrator.getEstimatedCovariance());
30729 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30730 assertNotNull(calibrator.getGroundTruthGravityNorm());
30731 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30732 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30733 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30734 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30735 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30736 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30737 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30738
30739
30740 final Acceleration invalidGravityNorm = new Acceleration(
30741 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30742
30743 calibrator = null;
30744 try {
30745 calibrator = new KnownGravityNormAccelerometerCalibrator(
30746 invalidGravityNorm, ba, ma, this);
30747 fail("IllegalArgumentException expected but not thrown");
30748 } catch (final IllegalArgumentException ignore) {
30749 }
30750 try {
30751 calibrator = new KnownGravityNormAccelerometerCalibrator(
30752 gravityNorm, new Matrix(1, 1), ma,
30753 this);
30754 fail("IllegalArgumentException expected but not thrown");
30755 } catch (final IllegalArgumentException ignore) {
30756 }
30757 try {
30758 calibrator = new KnownGravityNormAccelerometerCalibrator(
30759 gravityNorm, new Matrix(1, 3), ma,
30760 this);
30761 fail("IllegalArgumentException expected but not thrown");
30762 } catch (final IllegalArgumentException ignore) {
30763 }
30764 try {
30765 calibrator = new KnownGravityNormAccelerometerCalibrator(
30766 gravityNorm, ba, new Matrix(1, 3),
30767 this);
30768 fail("IllegalArgumentException expected but not thrown");
30769 } catch (final IllegalArgumentException ignore) {
30770 }
30771 try {
30772 calibrator = new KnownGravityNormAccelerometerCalibrator(
30773 gravityNorm, ba, new Matrix(3, 1),
30774 this);
30775 fail("IllegalArgumentException expected but not thrown");
30776 } catch (final IllegalArgumentException ignore) {
30777 }
30778 assertNull(calibrator);
30779 }
30780
30781 @Test
30782 public void testConstructor229() throws WrongSizeException {
30783 final Collection<StandardDeviationBodyKinematics> measurements =
30784 Collections.emptyList();
30785
30786 final Matrix ba = generateBa();
30787 final double[] bias = ba.getBuffer();
30788 final double biasX = ba.getElementAtIndex(0);
30789 final double biasY = ba.getElementAtIndex(1);
30790 final double biasZ = ba.getElementAtIndex(2);
30791
30792 final Matrix ma = generateMaCommonAxis();
30793 final double sx = ma.getElementAt(0, 0);
30794 final double sy = ma.getElementAt(1, 1);
30795 final double sz = ma.getElementAt(2, 2);
30796 final double mxy = ma.getElementAt(0, 1);
30797 final double mxz = ma.getElementAt(0, 2);
30798 final double myx = ma.getElementAt(1, 0);
30799 final double myz = ma.getElementAt(1, 2);
30800 final double mzx = ma.getElementAt(2, 0);
30801 final double mzy = ma.getElementAt(2, 1);
30802
30803 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30804 final double latitude = Math.toRadians(
30805 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30806 final double longitude = Math.toRadians(
30807 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30808 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30809 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30810 final NEDVelocity nedVelocity = new NEDVelocity();
30811 final ECEFPosition ecefPosition = new ECEFPosition();
30812 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30813 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30814 ecefPosition, ecefVelocity);
30815 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30816 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30817 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30818
30819 KnownGravityNormAccelerometerCalibrator calibrator =
30820 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
30821 measurements, ba, ma);
30822
30823
30824 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30825 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30826 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30827 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30828 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30829 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30830 final Acceleration bx2 = new Acceleration(0.0,
30831 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30832 calibrator.getInitialBiasXAsAcceleration(bx2);
30833 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30834 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30835 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30836 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30837 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30838 final Acceleration by2 = new Acceleration(0.0,
30839 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30840 calibrator.getInitialBiasYAsAcceleration(by2);
30841 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30842 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30843 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30844 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30845 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30846 final Acceleration bz2 = new Acceleration(0.0,
30847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30848 calibrator.getInitialBiasZAsAcceleration(bz2);
30849 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30850 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30851 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30852 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30853 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30854 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30855 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30856 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30857 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30858 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30859 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30860 final double[] bias1 = calibrator.getInitialBias();
30861 assertArrayEquals(bias1, bias, 0.0);
30862 final double[] bias2 = new double[3];
30863 calibrator.getInitialBias(bias2);
30864 assertArrayEquals(bias1, bias2, 0.0);
30865 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30866 assertEquals(b1, ba);
30867 final Matrix b2 = new Matrix(3, 1);
30868 calibrator.getInitialBiasAsMatrix(b2);
30869 assertEquals(b1, b2);
30870 final Matrix ma1 = new Matrix(3, 3);
30871 ma1.setSubmatrix(0, 0,
30872 2, 2,
30873 new double[]{sx, myx, mzx,
30874 mxy, sy, mzy,
30875 mxz, myz, sz});
30876 assertEquals(calibrator.getInitialMa(), ma1);
30877 final Matrix ma2 = new Matrix(3, 3);
30878 calibrator.getInitialMa(ma2);
30879 assertEquals(ma1, ma2);
30880 assertSame(calibrator.getMeasurements(), measurements);
30881 assertFalse(calibrator.isCommonAxisUsed());
30882 assertNull(calibrator.getListener());
30883 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30884 assertFalse(calibrator.isReady());
30885 assertFalse(calibrator.isRunning());
30886 assertNull(calibrator.getEstimatedBiases());
30887 assertFalse(calibrator.getEstimatedBiases(null));
30888 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30889 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30890 assertNull(calibrator.getEstimatedBiasFx());
30891 assertNull(calibrator.getEstimatedBiasFy());
30892 assertNull(calibrator.getEstimatedBiasFz());
30893 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30894 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30895 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30896 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30897 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30898 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30899 assertNull(calibrator.getEstimatedMa());
30900 assertNull(calibrator.getEstimatedSx());
30901 assertNull(calibrator.getEstimatedSy());
30902 assertNull(calibrator.getEstimatedSz());
30903 assertNull(calibrator.getEstimatedMxy());
30904 assertNull(calibrator.getEstimatedMxz());
30905 assertNull(calibrator.getEstimatedMyx());
30906 assertNull(calibrator.getEstimatedMyz());
30907 assertNull(calibrator.getEstimatedMzx());
30908 assertNull(calibrator.getEstimatedMzy());
30909 assertNull(calibrator.getEstimatedCovariance());
30910 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30911 assertNotNull(calibrator.getGroundTruthGravityNorm());
30912 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30913 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30914 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30915 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30916 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30917 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30918 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30919
30920
30921 final Acceleration invalidGravityNorm = new Acceleration(
30922 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30923
30924 calibrator = null;
30925 try {
30926 calibrator = new KnownGravityNormAccelerometerCalibrator(
30927 invalidGravityNorm, ba, ma, this);
30928 fail("IllegalArgumentException expected but not thrown");
30929 } catch (final IllegalArgumentException ignore) {
30930 }
30931 try {
30932 calibrator = new KnownGravityNormAccelerometerCalibrator(
30933 gravityNorm, measurements,
30934 new Matrix(1, 1), ma);
30935 fail("IllegalArgumentException expected but not thrown");
30936 } catch (final IllegalArgumentException ignore) {
30937 }
30938 try {
30939 calibrator = new KnownGravityNormAccelerometerCalibrator(
30940 gravityNorm, measurements,
30941 new Matrix(1, 3), ma);
30942 fail("IllegalArgumentException expected but not thrown");
30943 } catch (final IllegalArgumentException ignore) {
30944 }
30945 try {
30946 calibrator = new KnownGravityNormAccelerometerCalibrator(
30947 gravityNorm, measurements, ba,
30948 new Matrix(1, 3));
30949 fail("IllegalArgumentException expected but not thrown");
30950 } catch (final IllegalArgumentException ignore) {
30951 }
30952 try {
30953 calibrator = new KnownGravityNormAccelerometerCalibrator(
30954 gravityNorm, measurements, ba,
30955 new Matrix(3, 1));
30956 fail("IllegalArgumentException expected but not thrown");
30957 } catch (final IllegalArgumentException ignore) {
30958 }
30959 assertNull(calibrator);
30960 }
30961
30962 @Test
30963 public void testConstructor230() throws WrongSizeException {
30964 final Collection<StandardDeviationBodyKinematics> measurements =
30965 Collections.emptyList();
30966
30967 final Matrix ba = generateBa();
30968 final double[] bias = ba.getBuffer();
30969 final double biasX = ba.getElementAtIndex(0);
30970 final double biasY = ba.getElementAtIndex(1);
30971 final double biasZ = ba.getElementAtIndex(2);
30972
30973 final Matrix ma = generateMaCommonAxis();
30974 final double sx = ma.getElementAt(0, 0);
30975 final double sy = ma.getElementAt(1, 1);
30976 final double sz = ma.getElementAt(2, 2);
30977 final double mxy = ma.getElementAt(0, 1);
30978 final double mxz = ma.getElementAt(0, 2);
30979 final double myx = ma.getElementAt(1, 0);
30980 final double myz = ma.getElementAt(1, 2);
30981 final double mzx = ma.getElementAt(2, 0);
30982 final double mzy = ma.getElementAt(2, 1);
30983
30984 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30985 final double latitude = Math.toRadians(
30986 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30987 final double longitude = Math.toRadians(
30988 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30989 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30990 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30991 final NEDVelocity nedVelocity = new NEDVelocity();
30992 final ECEFPosition ecefPosition = new ECEFPosition();
30993 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30994 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30995 ecefPosition, ecefVelocity);
30996 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30997 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30998 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30999
31000 KnownGravityNormAccelerometerCalibrator calibrator =
31001 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31002 measurements, ba, ma, this);
31003
31004
31005 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31006 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31007 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31008 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31009 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31010 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31011 final Acceleration bx2 = new Acceleration(0.0,
31012 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31013 calibrator.getInitialBiasXAsAcceleration(bx2);
31014 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31015 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31016 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31017 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31018 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31019 final Acceleration by2 = new Acceleration(0.0,
31020 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31021 calibrator.getInitialBiasYAsAcceleration(by2);
31022 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31023 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31024 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31025 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31026 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31027 final Acceleration bz2 = new Acceleration(0.0,
31028 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31029 calibrator.getInitialBiasZAsAcceleration(bz2);
31030 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31031 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31032 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31033 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31034 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31035 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31036 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31037 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31038 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31039 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31040 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31041 final double[] bias1 = calibrator.getInitialBias();
31042 assertArrayEquals(bias1, bias, 0.0);
31043 final double[] bias2 = new double[3];
31044 calibrator.getInitialBias(bias2);
31045 assertArrayEquals(bias1, bias2, 0.0);
31046 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31047 assertEquals(b1, ba);
31048 final Matrix b2 = new Matrix(3, 1);
31049 calibrator.getInitialBiasAsMatrix(b2);
31050 assertEquals(b1, b2);
31051 final Matrix ma1 = new Matrix(3, 3);
31052 ma1.setSubmatrix(0, 0,
31053 2, 2,
31054 new double[]{sx, myx, mzx,
31055 mxy, sy, mzy,
31056 mxz, myz, sz});
31057 assertEquals(calibrator.getInitialMa(), ma1);
31058 final Matrix ma2 = new Matrix(3, 3);
31059 calibrator.getInitialMa(ma2);
31060 assertEquals(ma1, ma2);
31061 assertSame(calibrator.getMeasurements(), measurements);
31062 assertFalse(calibrator.isCommonAxisUsed());
31063 assertSame(calibrator.getListener(), this);
31064 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
31065 assertFalse(calibrator.isReady());
31066 assertFalse(calibrator.isRunning());
31067 assertNull(calibrator.getEstimatedBiases());
31068 assertFalse(calibrator.getEstimatedBiases(null));
31069 assertNull(calibrator.getEstimatedBiasesAsMatrix());
31070 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31071 assertNull(calibrator.getEstimatedBiasFx());
31072 assertNull(calibrator.getEstimatedBiasFy());
31073 assertNull(calibrator.getEstimatedBiasFz());
31074 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31075 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31076 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31077 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31078 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31079 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31080 assertNull(calibrator.getEstimatedMa());
31081 assertNull(calibrator.getEstimatedSx());
31082 assertNull(calibrator.getEstimatedSy());
31083 assertNull(calibrator.getEstimatedSz());
31084 assertNull(calibrator.getEstimatedMxy());
31085 assertNull(calibrator.getEstimatedMxz());
31086 assertNull(calibrator.getEstimatedMyx());
31087 assertNull(calibrator.getEstimatedMyz());
31088 assertNull(calibrator.getEstimatedMzx());
31089 assertNull(calibrator.getEstimatedMzy());
31090 assertNull(calibrator.getEstimatedCovariance());
31091 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31092 assertNotNull(calibrator.getGroundTruthGravityNorm());
31093 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31094 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31095 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31096 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31097 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31098 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31099 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31100
31101
31102 final Acceleration invalidGravityNorm = new Acceleration(
31103 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31104
31105 calibrator = null;
31106 try {
31107 calibrator = new KnownGravityNormAccelerometerCalibrator(
31108 invalidGravityNorm, ba, ma, this);
31109 fail("IllegalArgumentException expected but not thrown");
31110 } catch (final IllegalArgumentException ignore) {
31111 }
31112 try {
31113 calibrator = new KnownGravityNormAccelerometerCalibrator(
31114 gravityNorm, measurements,
31115 new Matrix(1, 1), ma, this);
31116 fail("IllegalArgumentException expected but not thrown");
31117 } catch (final IllegalArgumentException ignore) {
31118 }
31119 try {
31120 calibrator = new KnownGravityNormAccelerometerCalibrator(
31121 gravityNorm, measurements,
31122 new Matrix(1, 3), ma, this);
31123 fail("IllegalArgumentException expected but not thrown");
31124 } catch (final IllegalArgumentException ignore) {
31125 }
31126 try {
31127 calibrator = new KnownGravityNormAccelerometerCalibrator(
31128 gravityNorm, measurements, ba,
31129 new Matrix(1, 3), this);
31130 fail("IllegalArgumentException expected but not thrown");
31131 } catch (final IllegalArgumentException ignore) {
31132 }
31133 try {
31134 calibrator = new KnownGravityNormAccelerometerCalibrator(
31135 gravityNorm, measurements, ba,
31136 new Matrix(3, 1), this);
31137 fail("IllegalArgumentException expected but not thrown");
31138 } catch (final IllegalArgumentException ignore) {
31139 }
31140 assertNull(calibrator);
31141 }
31142
31143 @Test
31144 public void testConstructor231() throws WrongSizeException {
31145 final Matrix ba = generateBa();
31146 final double[] bias = ba.getBuffer();
31147 final double biasX = ba.getElementAtIndex(0);
31148 final double biasY = ba.getElementAtIndex(1);
31149 final double biasZ = ba.getElementAtIndex(2);
31150
31151 final Matrix ma = generateMaCommonAxis();
31152 final double sx = ma.getElementAt(0, 0);
31153 final double sy = ma.getElementAt(1, 1);
31154 final double sz = ma.getElementAt(2, 2);
31155 final double mxy = ma.getElementAt(0, 1);
31156 final double mxz = ma.getElementAt(0, 2);
31157 final double myx = ma.getElementAt(1, 0);
31158 final double myz = ma.getElementAt(1, 2);
31159 final double mzx = ma.getElementAt(2, 0);
31160 final double mzy = ma.getElementAt(2, 1);
31161
31162 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31163 final double latitude = Math.toRadians(
31164 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31165 final double longitude = Math.toRadians(
31166 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31167 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31168 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31169 final NEDVelocity nedVelocity = new NEDVelocity();
31170 final ECEFPosition ecefPosition = new ECEFPosition();
31171 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31172 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31173 ecefPosition, ecefVelocity);
31174 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31175 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31176 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31177
31178 KnownGravityNormAccelerometerCalibrator calibrator =
31179 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31180 true, ba, ma);
31181
31182
31183 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31184 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31185 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31186 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31187 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31188 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31189 final Acceleration bx2 = new Acceleration(0.0,
31190 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31191 calibrator.getInitialBiasXAsAcceleration(bx2);
31192 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31193 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31194 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31195 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31196 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31197 final Acceleration by2 = new Acceleration(0.0,
31198 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31199 calibrator.getInitialBiasYAsAcceleration(by2);
31200 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31201 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31202 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31203 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31204 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31205 final Acceleration bz2 = new Acceleration(0.0,
31206 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31207 calibrator.getInitialBiasZAsAcceleration(bz2);
31208 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31209 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31210 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31211 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31212 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31213 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31214 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31215 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31216 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31217 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31218 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31219 final double[] bias1 = calibrator.getInitialBias();
31220 assertArrayEquals(bias1, bias, 0.0);
31221 final double[] bias2 = new double[3];
31222 calibrator.getInitialBias(bias2);
31223 assertArrayEquals(bias1, bias2, 0.0);
31224 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31225 assertEquals(b1, ba);
31226 final Matrix b2 = new Matrix(3, 1);
31227 calibrator.getInitialBiasAsMatrix(b2);
31228 assertEquals(b1, b2);
31229 final Matrix ma1 = new Matrix(3, 3);
31230 ma1.setSubmatrix(0, 0,
31231 2, 2,
31232 new double[]{sx, myx, mzx,
31233 mxy, sy, mzy,
31234 mxz, myz, sz});
31235 assertEquals(calibrator.getInitialMa(), ma1);
31236 final Matrix ma2 = new Matrix(3, 3);
31237 calibrator.getInitialMa(ma2);
31238 assertEquals(ma1, ma2);
31239 assertNull(calibrator.getMeasurements());
31240 assertTrue(calibrator.isCommonAxisUsed());
31241 assertNull(calibrator.getListener());
31242 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31243 assertFalse(calibrator.isReady());
31244 assertFalse(calibrator.isRunning());
31245 assertNull(calibrator.getEstimatedBiases());
31246 assertFalse(calibrator.getEstimatedBiases(null));
31247 assertNull(calibrator.getEstimatedBiasesAsMatrix());
31248 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31249 assertNull(calibrator.getEstimatedBiasFx());
31250 assertNull(calibrator.getEstimatedBiasFy());
31251 assertNull(calibrator.getEstimatedBiasFz());
31252 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31253 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31254 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31255 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31256 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31257 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31258 assertNull(calibrator.getEstimatedMa());
31259 assertNull(calibrator.getEstimatedSx());
31260 assertNull(calibrator.getEstimatedSy());
31261 assertNull(calibrator.getEstimatedSz());
31262 assertNull(calibrator.getEstimatedMxy());
31263 assertNull(calibrator.getEstimatedMxz());
31264 assertNull(calibrator.getEstimatedMyx());
31265 assertNull(calibrator.getEstimatedMyz());
31266 assertNull(calibrator.getEstimatedMzx());
31267 assertNull(calibrator.getEstimatedMzy());
31268 assertNull(calibrator.getEstimatedCovariance());
31269 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31270 assertNotNull(calibrator.getGroundTruthGravityNorm());
31271 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31272 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31273 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31274 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31275 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31276 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31277 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31278
31279
31280 final Acceleration invalidGravityNorm = new Acceleration(
31281 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31282
31283 calibrator = null;
31284 try {
31285 calibrator = new KnownGravityNormAccelerometerCalibrator(
31286 invalidGravityNorm, true, ba, ma);
31287 fail("IllegalArgumentException expected but not thrown");
31288 } catch (final IllegalArgumentException ignore) {
31289 }
31290 try {
31291 calibrator = new KnownGravityNormAccelerometerCalibrator(
31292 gravityNorm, true,
31293 new Matrix(1, 1), ma);
31294 fail("IllegalArgumentException expected but not thrown");
31295 } catch (final IllegalArgumentException ignore) {
31296 }
31297 try {
31298 calibrator = new KnownGravityNormAccelerometerCalibrator(
31299 gravityNorm, true,
31300 new Matrix(1, 3), ma);
31301 fail("IllegalArgumentException expected but not thrown");
31302 } catch (final IllegalArgumentException ignore) {
31303 }
31304 try {
31305 calibrator = new KnownGravityNormAccelerometerCalibrator(
31306 gravityNorm, true, ba,
31307 new Matrix(1, 3));
31308 fail("IllegalArgumentException expected but not thrown");
31309 } catch (final IllegalArgumentException ignore) {
31310 }
31311 try {
31312 calibrator = new KnownGravityNormAccelerometerCalibrator(
31313 gravityNorm, true, ba,
31314 new Matrix(3, 1));
31315 fail("IllegalArgumentException expected but not thrown");
31316 } catch (final IllegalArgumentException ignore) {
31317 }
31318 assertNull(calibrator);
31319 }
31320
31321 @Test
31322 public void testConstructor232() throws WrongSizeException {
31323 final Matrix ba = generateBa();
31324 final double[] bias = ba.getBuffer();
31325 final double biasX = ba.getElementAtIndex(0);
31326 final double biasY = ba.getElementAtIndex(1);
31327 final double biasZ = ba.getElementAtIndex(2);
31328
31329 final Matrix ma = generateMaCommonAxis();
31330 final double sx = ma.getElementAt(0, 0);
31331 final double sy = ma.getElementAt(1, 1);
31332 final double sz = ma.getElementAt(2, 2);
31333 final double mxy = ma.getElementAt(0, 1);
31334 final double mxz = ma.getElementAt(0, 2);
31335 final double myx = ma.getElementAt(1, 0);
31336 final double myz = ma.getElementAt(1, 2);
31337 final double mzx = ma.getElementAt(2, 0);
31338 final double mzy = ma.getElementAt(2, 1);
31339
31340 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31341 final double latitude = Math.toRadians(
31342 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31343 final double longitude = Math.toRadians(
31344 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31345 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31346 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31347 final NEDVelocity nedVelocity = new NEDVelocity();
31348 final ECEFPosition ecefPosition = new ECEFPosition();
31349 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31350 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31351 ecefPosition, ecefVelocity);
31352 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31353 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31354 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31355
31356 KnownGravityNormAccelerometerCalibrator calibrator =
31357 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31358 true, ba, ma, this);
31359
31360
31361 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31362 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31363 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31364 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31365 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31366 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31367 final Acceleration bx2 = new Acceleration(0.0,
31368 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31369 calibrator.getInitialBiasXAsAcceleration(bx2);
31370 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31371 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31372 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31373 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31374 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31375 final Acceleration by2 = new Acceleration(0.0,
31376 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31377 calibrator.getInitialBiasYAsAcceleration(by2);
31378 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31379 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31380 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31381 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31382 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31383 final Acceleration bz2 = new Acceleration(0.0,
31384 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31385 calibrator.getInitialBiasZAsAcceleration(bz2);
31386 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31387 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31388 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31389 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31390 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31391 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31392 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31393 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31394 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31395 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31396 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31397 final double[] bias1 = calibrator.getInitialBias();
31398 assertArrayEquals(bias1, bias, 0.0);
31399 final double[] bias2 = new double[3];
31400 calibrator.getInitialBias(bias2);
31401 assertArrayEquals(bias1, bias2, 0.0);
31402 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31403 assertEquals(b1, ba);
31404 final Matrix b2 = new Matrix(3, 1);
31405 calibrator.getInitialBiasAsMatrix(b2);
31406 assertEquals(b1, b2);
31407 final Matrix ma1 = new Matrix(3, 3);
31408 ma1.setSubmatrix(0, 0,
31409 2, 2,
31410 new double[]{sx, myx, mzx,
31411 mxy, sy, mzy,
31412 mxz, myz, sz});
31413 assertEquals(calibrator.getInitialMa(), ma1);
31414 final Matrix ma2 = new Matrix(3, 3);
31415 calibrator.getInitialMa(ma2);
31416 assertEquals(ma1, ma2);
31417 assertNull(calibrator.getMeasurements());
31418 assertTrue(calibrator.isCommonAxisUsed());
31419 assertSame(calibrator.getListener(), this);
31420 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31421 assertFalse(calibrator.isReady());
31422 assertFalse(calibrator.isRunning());
31423 assertNull(calibrator.getEstimatedBiases());
31424 assertFalse(calibrator.getEstimatedBiases(null));
31425 assertNull(calibrator.getEstimatedBiasesAsMatrix());
31426 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31427 assertNull(calibrator.getEstimatedBiasFx());
31428 assertNull(calibrator.getEstimatedBiasFy());
31429 assertNull(calibrator.getEstimatedBiasFz());
31430 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31431 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31432 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31433 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31434 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31435 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31436 assertNull(calibrator.getEstimatedMa());
31437 assertNull(calibrator.getEstimatedSx());
31438 assertNull(calibrator.getEstimatedSy());
31439 assertNull(calibrator.getEstimatedSz());
31440 assertNull(calibrator.getEstimatedMxy());
31441 assertNull(calibrator.getEstimatedMxz());
31442 assertNull(calibrator.getEstimatedMyx());
31443 assertNull(calibrator.getEstimatedMyz());
31444 assertNull(calibrator.getEstimatedMzx());
31445 assertNull(calibrator.getEstimatedMzy());
31446 assertNull(calibrator.getEstimatedCovariance());
31447 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31448 assertNotNull(calibrator.getGroundTruthGravityNorm());
31449 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31450 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31451 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31452 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31453 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31454 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31455 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31456
31457
31458 final Acceleration invalidGravityNorm = new Acceleration(
31459 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31460
31461 calibrator = null;
31462 try {
31463 calibrator = new KnownGravityNormAccelerometerCalibrator(
31464 invalidGravityNorm, true, ba, ma);
31465 fail("IllegalArgumentException expected but not thrown");
31466 } catch (final IllegalArgumentException ignore) {
31467 }
31468 try {
31469 calibrator = new KnownGravityNormAccelerometerCalibrator(
31470 gravityNorm, true,
31471 new Matrix(1, 1), ma, this);
31472 fail("IllegalArgumentException expected but not thrown");
31473 } catch (final IllegalArgumentException ignore) {
31474 }
31475 try {
31476 calibrator = new KnownGravityNormAccelerometerCalibrator(
31477 gravityNorm, true,
31478 new Matrix(1, 3), ma, this);
31479 fail("IllegalArgumentException expected but not thrown");
31480 } catch (final IllegalArgumentException ignore) {
31481 }
31482 try {
31483 calibrator = new KnownGravityNormAccelerometerCalibrator(
31484 gravityNorm, true, ba,
31485 new Matrix(1, 3), this);
31486 fail("IllegalArgumentException expected but not thrown");
31487 } catch (final IllegalArgumentException ignore) {
31488 }
31489 try {
31490 calibrator = new KnownGravityNormAccelerometerCalibrator(
31491 gravityNorm, true, ba,
31492 new Matrix(3, 1), this);
31493 fail("IllegalArgumentException expected but not thrown");
31494 } catch (final IllegalArgumentException ignore) {
31495 }
31496 assertNull(calibrator);
31497 }
31498
31499 @Test
31500 public void testConstructor233() throws WrongSizeException {
31501 final Collection<StandardDeviationBodyKinematics> measurements =
31502 Collections.emptyList();
31503
31504 final Matrix ba = generateBa();
31505 final double[] bias = ba.getBuffer();
31506 final double biasX = ba.getElementAtIndex(0);
31507 final double biasY = ba.getElementAtIndex(1);
31508 final double biasZ = ba.getElementAtIndex(2);
31509
31510 final Matrix ma = generateMaCommonAxis();
31511 final double sx = ma.getElementAt(0, 0);
31512 final double sy = ma.getElementAt(1, 1);
31513 final double sz = ma.getElementAt(2, 2);
31514 final double mxy = ma.getElementAt(0, 1);
31515 final double mxz = ma.getElementAt(0, 2);
31516 final double myx = ma.getElementAt(1, 0);
31517 final double myz = ma.getElementAt(1, 2);
31518 final double mzx = ma.getElementAt(2, 0);
31519 final double mzy = ma.getElementAt(2, 1);
31520
31521 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31522 final double latitude = Math.toRadians(
31523 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31524 final double longitude = Math.toRadians(
31525 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31526 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31527 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31528 final NEDVelocity nedVelocity = new NEDVelocity();
31529 final ECEFPosition ecefPosition = new ECEFPosition();
31530 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31531 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31532 ecefPosition, ecefVelocity);
31533 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31534 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31535 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31536
31537 KnownGravityNormAccelerometerCalibrator calibrator =
31538 new KnownGravityNormAccelerometerCalibrator(
31539 gravityNorm, measurements, true,
31540 ba, ma);
31541
31542
31543 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31544 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31545 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31546 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31547 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31548 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31549 final Acceleration bx2 = new Acceleration(0.0,
31550 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31551 calibrator.getInitialBiasXAsAcceleration(bx2);
31552 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31553 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31554 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31555 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31556 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31557 final Acceleration by2 = new Acceleration(0.0,
31558 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31559 calibrator.getInitialBiasYAsAcceleration(by2);
31560 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31561 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31562 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31563 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31564 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31565 final Acceleration bz2 = new Acceleration(0.0,
31566 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31567 calibrator.getInitialBiasZAsAcceleration(bz2);
31568 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31569 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31570 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31571 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31572 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31573 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31574 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31575 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31576 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31577 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31578 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31579 final double[] bias1 = calibrator.getInitialBias();
31580 assertArrayEquals(bias1, bias, 0.0);
31581 final double[] bias2 = new double[3];
31582 calibrator.getInitialBias(bias2);
31583 assertArrayEquals(bias1, bias2, 0.0);
31584 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31585 assertEquals(b1, ba);
31586 final Matrix b2 = new Matrix(3, 1);
31587 calibrator.getInitialBiasAsMatrix(b2);
31588 assertEquals(b1, b2);
31589 final Matrix ma1 = new Matrix(3, 3);
31590 ma1.setSubmatrix(0, 0,
31591 2, 2,
31592 new double[]{sx, myx, mzx,
31593 mxy, sy, mzy,
31594 mxz, myz, sz});
31595 assertEquals(calibrator.getInitialMa(), ma1);
31596 final Matrix ma2 = new Matrix(3, 3);
31597 calibrator.getInitialMa(ma2);
31598 assertEquals(ma1, ma2);
31599 assertSame(calibrator.getMeasurements(), measurements);
31600 assertTrue(calibrator.isCommonAxisUsed());
31601 assertNull(calibrator.getListener());
31602 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31603 assertFalse(calibrator.isReady());
31604 assertFalse(calibrator.isRunning());
31605 assertNull(calibrator.getEstimatedBiases());
31606 assertFalse(calibrator.getEstimatedBiases(null));
31607 assertNull(calibrator.getEstimatedBiasesAsMatrix());
31608 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31609 assertNull(calibrator.getEstimatedBiasFx());
31610 assertNull(calibrator.getEstimatedBiasFy());
31611 assertNull(calibrator.getEstimatedBiasFz());
31612 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31613 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31614 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31615 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31616 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31617 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31618 assertNull(calibrator.getEstimatedMa());
31619 assertNull(calibrator.getEstimatedSx());
31620 assertNull(calibrator.getEstimatedSy());
31621 assertNull(calibrator.getEstimatedSz());
31622 assertNull(calibrator.getEstimatedMxy());
31623 assertNull(calibrator.getEstimatedMxz());
31624 assertNull(calibrator.getEstimatedMyx());
31625 assertNull(calibrator.getEstimatedMyz());
31626 assertNull(calibrator.getEstimatedMzx());
31627 assertNull(calibrator.getEstimatedMzy());
31628 assertNull(calibrator.getEstimatedCovariance());
31629 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31630 assertNotNull(calibrator.getGroundTruthGravityNorm());
31631 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31632 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31633 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31634 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31635 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31636 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31637 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31638
31639
31640 final Acceleration invalidGravityNorm = new Acceleration(
31641 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31642
31643 calibrator = null;
31644 try {
31645 calibrator = new KnownGravityNormAccelerometerCalibrator(
31646 invalidGravityNorm, measurements, true,
31647 ba, ma);
31648 fail("IllegalArgumentException expected but not thrown");
31649 } catch (final IllegalArgumentException ignore) {
31650 }
31651 try {
31652 calibrator = new KnownGravityNormAccelerometerCalibrator(
31653 gravityNorm, measurements, true,
31654 new Matrix(1, 1), ma);
31655 fail("IllegalArgumentException expected but not thrown");
31656 } catch (final IllegalArgumentException ignore) {
31657 }
31658 try {
31659 calibrator = new KnownGravityNormAccelerometerCalibrator(
31660 gravityNorm, measurements, true,
31661 new Matrix(1, 3), ma);
31662 fail("IllegalArgumentException expected but not thrown");
31663 } catch (final IllegalArgumentException ignore) {
31664 }
31665 try {
31666 calibrator = new KnownGravityNormAccelerometerCalibrator(
31667 gravityNorm, measurements, true,
31668 ba, new Matrix(1, 3));
31669 fail("IllegalArgumentException expected but not thrown");
31670 } catch (final IllegalArgumentException ignore) {
31671 }
31672 try {
31673 calibrator = new KnownGravityNormAccelerometerCalibrator(
31674 gravityNorm, measurements, true,
31675 ba, new Matrix(3, 1));
31676 fail("IllegalArgumentException expected but not thrown");
31677 } catch (final IllegalArgumentException ignore) {
31678 }
31679 assertNull(calibrator);
31680 }
31681
31682 @Test
31683 public void testConstructor234() throws WrongSizeException {
31684 final Collection<StandardDeviationBodyKinematics> measurements =
31685 Collections.emptyList();
31686
31687 final Matrix ba = generateBa();
31688 final double[] bias = ba.getBuffer();
31689 final double biasX = ba.getElementAtIndex(0);
31690 final double biasY = ba.getElementAtIndex(1);
31691 final double biasZ = ba.getElementAtIndex(2);
31692
31693 final Matrix ma = generateMaCommonAxis();
31694 final double sx = ma.getElementAt(0, 0);
31695 final double sy = ma.getElementAt(1, 1);
31696 final double sz = ma.getElementAt(2, 2);
31697 final double mxy = ma.getElementAt(0, 1);
31698 final double mxz = ma.getElementAt(0, 2);
31699 final double myx = ma.getElementAt(1, 0);
31700 final double myz = ma.getElementAt(1, 2);
31701 final double mzx = ma.getElementAt(2, 0);
31702 final double mzy = ma.getElementAt(2, 1);
31703
31704 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31705 final double latitude = Math.toRadians(
31706 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31707 final double longitude = Math.toRadians(
31708 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31709 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31710 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31711 final NEDVelocity nedVelocity = new NEDVelocity();
31712 final ECEFPosition ecefPosition = new ECEFPosition();
31713 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31714 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31715 ecefPosition, ecefVelocity);
31716 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31717 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31718 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31719
31720 KnownGravityNormAccelerometerCalibrator calibrator =
31721 new KnownGravityNormAccelerometerCalibrator(
31722 gravityNorm, measurements,
31723 true, ba, ma, this);
31724
31725
31726 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31727 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31728 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31729 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31730 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31731 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31732 final Acceleration bx2 = new Acceleration(0.0,
31733 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31734 calibrator.getInitialBiasXAsAcceleration(bx2);
31735 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31736 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31737 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31738 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31739 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31740 final Acceleration by2 = new Acceleration(0.0,
31741 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31742 calibrator.getInitialBiasYAsAcceleration(by2);
31743 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31744 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31745 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31746 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31747 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31748 final Acceleration bz2 = new Acceleration(0.0,
31749 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31750 calibrator.getInitialBiasZAsAcceleration(bz2);
31751 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31752 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31753 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31754 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31755 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31756 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31757 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31758 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31759 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31760 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31761 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31762 final double[] bias1 = calibrator.getInitialBias();
31763 assertArrayEquals(bias1, bias, 0.0);
31764 final double[] bias2 = new double[3];
31765 calibrator.getInitialBias(bias2);
31766 assertArrayEquals(bias1, bias2, 0.0);
31767 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31768 assertEquals(b1, ba);
31769 final Matrix b2 = new Matrix(3, 1);
31770 calibrator.getInitialBiasAsMatrix(b2);
31771 assertEquals(b1, b2);
31772 final Matrix ma1 = new Matrix(3, 3);
31773 ma1.setSubmatrix(0, 0,
31774 2, 2,
31775 new double[]{sx, myx, mzx,
31776 mxy, sy, mzy,
31777 mxz, myz, sz});
31778 assertEquals(calibrator.getInitialMa(), ma1);
31779 final Matrix ma2 = new Matrix(3, 3);
31780 calibrator.getInitialMa(ma2);
31781 assertEquals(ma1, ma2);
31782 assertSame(calibrator.getMeasurements(), measurements);
31783 assertTrue(calibrator.isCommonAxisUsed());
31784 assertSame(calibrator.getListener(), this);
31785 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31786 assertFalse(calibrator.isReady());
31787 assertFalse(calibrator.isRunning());
31788 assertNull(calibrator.getEstimatedBiases());
31789 assertFalse(calibrator.getEstimatedBiases(null));
31790 assertNull(calibrator.getEstimatedBiasesAsMatrix());
31791 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31792 assertNull(calibrator.getEstimatedBiasFx());
31793 assertNull(calibrator.getEstimatedBiasFy());
31794 assertNull(calibrator.getEstimatedBiasFz());
31795 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31796 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31797 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31798 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31799 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31800 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31801 assertNull(calibrator.getEstimatedMa());
31802 assertNull(calibrator.getEstimatedSx());
31803 assertNull(calibrator.getEstimatedSy());
31804 assertNull(calibrator.getEstimatedSz());
31805 assertNull(calibrator.getEstimatedMxy());
31806 assertNull(calibrator.getEstimatedMxz());
31807 assertNull(calibrator.getEstimatedMyx());
31808 assertNull(calibrator.getEstimatedMyz());
31809 assertNull(calibrator.getEstimatedMzx());
31810 assertNull(calibrator.getEstimatedMzy());
31811 assertNull(calibrator.getEstimatedCovariance());
31812 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31813 assertNotNull(calibrator.getGroundTruthGravityNorm());
31814 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31815 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31816 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31817 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31818 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31819 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31820 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31821
31822
31823 final Acceleration invalidGravityNorm = new Acceleration(
31824 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31825
31826 calibrator = null;
31827 try {
31828 calibrator = new KnownGravityNormAccelerometerCalibrator(
31829 invalidGravityNorm, measurements,
31830 true, ba, ma, this);
31831 fail("IllegalArgumentException expected but not thrown");
31832 } catch (final IllegalArgumentException ignore) {
31833 }
31834 try {
31835 calibrator = new KnownGravityNormAccelerometerCalibrator(
31836 gravityNorm, measurements, true,
31837 new Matrix(1, 1), ma, this);
31838 fail("IllegalArgumentException expected but not thrown");
31839 } catch (final IllegalArgumentException ignore) {
31840 }
31841 try {
31842 calibrator = new KnownGravityNormAccelerometerCalibrator(
31843 gravityNorm, measurements, true,
31844 new Matrix(1, 3), ma, this);
31845 fail("IllegalArgumentException expected but not thrown");
31846 } catch (final IllegalArgumentException ignore) {
31847 }
31848 try {
31849 calibrator = new KnownGravityNormAccelerometerCalibrator(
31850 gravityNorm, measurements, true,
31851 ba, new Matrix(1, 3), this);
31852 fail("IllegalArgumentException expected but not thrown");
31853 } catch (final IllegalArgumentException ignore) {
31854 }
31855 try {
31856 calibrator = new KnownGravityNormAccelerometerCalibrator(
31857 gravityNorm, measurements, true,
31858 ba, new Matrix(3, 1), this);
31859 fail("IllegalArgumentException expected but not thrown");
31860 } catch (final IllegalArgumentException ignore) {
31861 }
31862 assertNull(calibrator);
31863 }
31864
31865 @Test
31866 public void testGetSetInitialBiasX() throws LockedException {
31867 final KnownGravityNormAccelerometerCalibrator calibrator =
31868 new KnownGravityNormAccelerometerCalibrator();
31869
31870 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31871
31872
31873 final Matrix ba = generateBa();
31874 final double biasX = ba.getElementAtIndex(0);
31875
31876 calibrator.setInitialBiasX(biasX);
31877
31878
31879 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31880 }
31881
31882 @Test
31883 public void testGetSetInitialBiasY() throws LockedException {
31884 final KnownGravityNormAccelerometerCalibrator calibrator =
31885 new KnownGravityNormAccelerometerCalibrator();
31886
31887 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31888
31889
31890 final Matrix ba = generateBa();
31891 final double biasY = ba.getElementAtIndex(1);
31892
31893 calibrator.setInitialBiasY(biasY);
31894
31895
31896 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31897 }
31898
31899 @Test
31900 public void testGetSetInitialBiasZ() throws LockedException {
31901 final KnownGravityNormAccelerometerCalibrator calibrator =
31902 new KnownGravityNormAccelerometerCalibrator();
31903
31904 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31905
31906
31907 final Matrix ba = generateBa();
31908 final double biasZ = ba.getElementAtIndex(2);
31909
31910 calibrator.setInitialBiasZ(biasZ);
31911
31912
31913 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31914 }
31915
31916 @Test
31917 public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
31918 final KnownGravityNormAccelerometerCalibrator calibrator =
31919 new KnownGravityNormAccelerometerCalibrator();
31920
31921
31922 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31923
31924 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
31925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31926
31927
31928 final Matrix ba = generateBa();
31929 final double biasX = ba.getElementAtIndex(0);
31930
31931 final Acceleration bx2 = new Acceleration(biasX,
31932 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31933 calibrator.setInitialBiasX(bx2);
31934
31935
31936 final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
31937 final Acceleration bx4 = new Acceleration(biasX,
31938 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31939 calibrator.getInitialBiasXAsAcceleration(bx4);
31940
31941 assertEquals(bx3, bx4);
31942 }
31943
31944 @Test
31945 public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
31946 final KnownGravityNormAccelerometerCalibrator calibrator =
31947 new KnownGravityNormAccelerometerCalibrator();
31948
31949
31950 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31951
31952 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
31953 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31954
31955
31956 final Matrix ba = generateBa();
31957 final double biasY = ba.getElementAtIndex(1);
31958
31959 final Acceleration by2 = new Acceleration(biasY,
31960 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31961 calibrator.setInitialBiasY(by2);
31962
31963
31964 final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
31965 final Acceleration by4 = new Acceleration(biasY,
31966 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31967 calibrator.getInitialBiasYAsAcceleration(by4);
31968
31969 assertEquals(by3, by4);
31970 }
31971
31972 @Test
31973 public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
31974 final KnownGravityNormAccelerometerCalibrator calibrator =
31975 new KnownGravityNormAccelerometerCalibrator();
31976
31977
31978 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31979
31980 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
31981 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31982
31983
31984 final Matrix ba = generateBa();
31985 final double biasZ = ba.getElementAtIndex(2);
31986
31987 final Acceleration bz2 = new Acceleration(biasZ,
31988 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31989 calibrator.setInitialBiasZ(bz2);
31990
31991
31992 final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
31993 final Acceleration bz4 = new Acceleration(biasZ,
31994 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31995 calibrator.getInitialBiasZAsAcceleration(bz4);
31996
31997 assertEquals(bz3, bz4);
31998 }
31999
32000 @Test
32001 public void testSetInitialBias1() throws LockedException {
32002 final KnownGravityNormAccelerometerCalibrator calibrator =
32003 new KnownGravityNormAccelerometerCalibrator();
32004
32005
32006 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
32007 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
32008 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
32009
32010
32011 final Matrix ba = generateBa();
32012 final double biasX = ba.getElementAtIndex(0);
32013 final double biasY = ba.getElementAtIndex(1);
32014 final double biasZ = ba.getElementAtIndex(2);
32015
32016 calibrator.setInitialBias(biasX, biasY, biasZ);
32017
32018
32019 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
32020 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
32021 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
32022 }
32023
32024 @Test
32025 public void testSetInitialBias2() throws LockedException {
32026 final KnownGravityNormAccelerometerCalibrator calibrator =
32027 new KnownGravityNormAccelerometerCalibrator();
32028
32029
32030 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
32031 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
32032 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
32033
32034
32035 final Matrix ba = generateBa();
32036 final double biasX = ba.getElementAtIndex(0);
32037 final double biasY = ba.getElementAtIndex(1);
32038 final double biasZ = ba.getElementAtIndex(2);
32039
32040 final Acceleration bx = new Acceleration(biasX,
32041 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32042 final Acceleration by = new Acceleration(biasY,
32043 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32044 final Acceleration bz = new Acceleration(biasZ,
32045 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32046
32047 calibrator.setInitialBias(bx, by, bz);
32048
32049
32050 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
32051 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
32052 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
32053 }
32054
32055 @Test
32056 public void testGetSetInitialSx() throws WrongSizeException, LockedException {
32057 final KnownGravityNormAccelerometerCalibrator calibrator =
32058 new KnownGravityNormAccelerometerCalibrator();
32059
32060 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32061
32062
32063 final Matrix ma = generateMaGeneral();
32064 final double sx = ma.getElementAt(0, 0);
32065
32066 calibrator.setInitialSx(sx);
32067
32068
32069 assertEquals(calibrator.getInitialSx(), sx, 0.0);
32070 }
32071
32072 @Test
32073 public void testGetSetInitialSy() throws WrongSizeException, LockedException {
32074 final KnownGravityNormAccelerometerCalibrator calibrator =
32075 new KnownGravityNormAccelerometerCalibrator();
32076
32077 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32078
32079
32080 final Matrix ma = generateMaGeneral();
32081 final double sy = ma.getElementAt(1, 1);
32082
32083 calibrator.setInitialSy(sy);
32084
32085
32086 assertEquals(calibrator.getInitialSy(), sy, 0.0);
32087 }
32088
32089 @Test
32090 public void testGetSetInitialSz() throws WrongSizeException, LockedException {
32091 final KnownGravityNormAccelerometerCalibrator calibrator =
32092 new KnownGravityNormAccelerometerCalibrator();
32093
32094 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32095
32096
32097 final Matrix ma = generateMaGeneral();
32098 final double sz = ma.getElementAt(2, 2);
32099
32100 calibrator.setInitialSz(sz);
32101
32102
32103 assertEquals(calibrator.getInitialSz(), sz, 0.0);
32104 }
32105
32106 @Test
32107 public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
32108 final KnownGravityNormAccelerometerCalibrator calibrator =
32109 new KnownGravityNormAccelerometerCalibrator();
32110
32111 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32112
32113
32114 final Matrix ma = generateMaGeneral();
32115 final double mxy = ma.getElementAt(0, 1);
32116
32117 calibrator.setInitialMxy(mxy);
32118
32119
32120 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32121 }
32122
32123 @Test
32124 public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
32125 final KnownGravityNormAccelerometerCalibrator calibrator =
32126 new KnownGravityNormAccelerometerCalibrator();
32127
32128 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32129
32130
32131 final Matrix ma = generateMaGeneral();
32132 final double mxz = ma.getElementAt(0, 2);
32133
32134 calibrator.setInitialMxz(mxz);
32135
32136
32137 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32138 }
32139
32140 @Test
32141 public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
32142 final KnownGravityNormAccelerometerCalibrator calibrator =
32143 new KnownGravityNormAccelerometerCalibrator();
32144
32145 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32146
32147
32148 final Matrix ma = generateMaGeneral();
32149 final double myx = ma.getElementAt(1, 0);
32150
32151 calibrator.setInitialMyx(myx);
32152
32153
32154 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32155 }
32156
32157 @Test
32158 public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
32159 final KnownGravityNormAccelerometerCalibrator calibrator =
32160 new KnownGravityNormAccelerometerCalibrator();
32161
32162 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32163
32164
32165 final Matrix ma = generateMaGeneral();
32166 final double myz = ma.getElementAt(1, 2);
32167
32168 calibrator.setInitialMyz(myz);
32169
32170
32171 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32172 }
32173
32174 @Test
32175 public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
32176 final KnownGravityNormAccelerometerCalibrator calibrator =
32177 new KnownGravityNormAccelerometerCalibrator();
32178
32179 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32180
32181
32182 final Matrix ma = generateMaGeneral();
32183 final double mzx = ma.getElementAt(2, 0);
32184
32185 calibrator.setInitialMzx(mzx);
32186
32187
32188 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32189 }
32190
32191 @Test
32192 public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
32193 final KnownGravityNormAccelerometerCalibrator calibrator =
32194 new KnownGravityNormAccelerometerCalibrator();
32195
32196 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32197
32198
32199 final Matrix ma = generateMaGeneral();
32200 final double mzy = ma.getElementAt(2, 1);
32201
32202 calibrator.setInitialMzy(mzy);
32203
32204
32205 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32206 }
32207
32208 @Test
32209 public void testSetInitialScalingFactors()
32210 throws WrongSizeException, LockedException {
32211
32212 final KnownGravityNormAccelerometerCalibrator calibrator =
32213 new KnownGravityNormAccelerometerCalibrator();
32214
32215
32216 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32217 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32218 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32219
32220
32221 final Matrix ma = generateMaGeneral();
32222 final double sx = ma.getElementAt(0, 0);
32223 final double sy = ma.getElementAt(1, 1);
32224 final double sz = ma.getElementAt(2, 2);
32225
32226 calibrator.setInitialScalingFactors(sx, sy, sz);
32227
32228
32229 assertEquals(calibrator.getInitialSx(), sx, 0.0);
32230 assertEquals(calibrator.getInitialSy(), sy, 0.0);
32231 assertEquals(calibrator.getInitialSz(), sz, 0.0);
32232 }
32233
32234 @Test
32235 public void testSetInitialCrossCouplingErrors()
32236 throws WrongSizeException, LockedException {
32237
32238 final KnownGravityNormAccelerometerCalibrator calibrator =
32239 new KnownGravityNormAccelerometerCalibrator();
32240
32241
32242 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32243 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32244 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32245 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32246 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32247 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32248
32249
32250 final Matrix ma = generateMaGeneral();
32251 final double mxy = ma.getElementAt(0, 1);
32252 final double mxz = ma.getElementAt(0, 2);
32253 final double myx = ma.getElementAt(1, 0);
32254 final double myz = ma.getElementAt(1, 2);
32255 final double mzx = ma.getElementAt(2, 0);
32256 final double mzy = ma.getElementAt(2, 1);
32257
32258 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
32259
32260
32261 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32262 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32263 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32264 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32265 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32266 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32267 }
32268
32269 @Test
32270 public void testSetInitialScalingFactorsAndCrossCouplingErrors()
32271 throws WrongSizeException, LockedException {
32272
32273 final KnownGravityNormAccelerometerCalibrator calibrator =
32274 new KnownGravityNormAccelerometerCalibrator();
32275
32276
32277 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32278 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32279 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32280 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32281 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32282 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32283 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32284 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32285 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32286
32287
32288 final Matrix ma = generateMaGeneral();
32289 final double sx = ma.getElementAt(0, 0);
32290 final double sy = ma.getElementAt(1, 1);
32291 final double sz = ma.getElementAt(2, 2);
32292 final double mxy = ma.getElementAt(0, 1);
32293 final double mxz = ma.getElementAt(0, 2);
32294 final double myx = ma.getElementAt(1, 0);
32295 final double myz = ma.getElementAt(1, 2);
32296 final double mzx = ma.getElementAt(2, 0);
32297 final double mzy = ma.getElementAt(2, 1);
32298
32299 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
32300 mxy, mxz, myx, myz, mzx, mzy);
32301
32302
32303 assertEquals(calibrator.getInitialSx(), sx, 0.0);
32304 assertEquals(calibrator.getInitialSy(), sy, 0.0);
32305 assertEquals(calibrator.getInitialSz(), sz, 0.0);
32306 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32307 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32308 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32309 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32310 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32311 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32312 }
32313
32314 @Test
32315 public void testGetSetInitialBias() throws LockedException {
32316 final KnownGravityNormAccelerometerCalibrator calibrator =
32317 new KnownGravityNormAccelerometerCalibrator();
32318
32319
32320 final double[] bias1 = calibrator.getInitialBias();
32321 assertArrayEquals(bias1, new double[3], 0.0);
32322
32323
32324 final Matrix ba = generateBa();
32325 final double[] bias2 = ba.getBuffer();
32326 calibrator.setInitialBias(bias2);
32327
32328
32329 final double[] bias3 = calibrator.getInitialBias();
32330 final double[] bias4 = new double[3];
32331 calibrator.getInitialBias(bias4);
32332
32333 assertArrayEquals(bias2, bias3, 0.0);
32334 assertArrayEquals(bias2, bias4, 0.0);
32335
32336
32337 try {
32338 calibrator.getInitialBias(new double[1]);
32339 fail("IllegalArgumentException expected but not thrown");
32340 } catch (final IllegalArgumentException ignore) {
32341 }
32342 try {
32343 calibrator.setInitialBias(new double[1]);
32344 fail("IllegalArgumentException expected but not thrown");
32345 } catch (final IllegalArgumentException ignore) {
32346 }
32347 }
32348
32349 @Test
32350 public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
32351 LockedException {
32352
32353 final KnownGravityNormAccelerometerCalibrator calibrator =
32354 new KnownGravityNormAccelerometerCalibrator();
32355
32356
32357 final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
32358 assertEquals(bias1, new Matrix(3, 1));
32359
32360
32361 final Matrix bias2 = generateBa();
32362 calibrator.setInitialBias(bias2);
32363
32364
32365 final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
32366 final Matrix bias4 = new Matrix(3, 1);
32367 calibrator.getInitialBiasAsMatrix(bias4);
32368
32369 assertEquals(bias2, bias3);
32370 assertEquals(bias2, bias4);
32371
32372
32373 try {
32374 calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
32375 fail("IllegalArgumentException expected but not thrown");
32376 } catch (final IllegalArgumentException ignore) {
32377 }
32378 try {
32379 calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
32380 fail("IllegalArgumentException expected but not thrown");
32381 } catch (final IllegalArgumentException ignore) {
32382 }
32383 try {
32384 calibrator.setInitialBias(new Matrix(1, 1));
32385 fail("IllegalArgumentException expected but not thrown");
32386 } catch (final IllegalArgumentException ignore) {
32387 }
32388 try {
32389 calibrator.setInitialBias(new Matrix(3, 3));
32390 fail("IllegalArgumentException expected but not thrown");
32391 } catch (final IllegalArgumentException ignore) {
32392 }
32393 }
32394
32395 @Test
32396 public void testGetSetInitialMa() throws WrongSizeException, LockedException {
32397 final KnownGravityNormAccelerometerCalibrator calibrator =
32398 new KnownGravityNormAccelerometerCalibrator();
32399
32400
32401 final Matrix ma1 = calibrator.getInitialMa();
32402 assertEquals(ma1, new Matrix(3, 3));
32403
32404
32405 final Matrix ma2 = generateMaGeneral();
32406 calibrator.setInitialMa(ma2);
32407
32408
32409 final Matrix ma3 = calibrator.getInitialMa();
32410 final Matrix ma4 = new Matrix(3, 3);
32411 calibrator.getInitialMa(ma4);
32412
32413 assertEquals(ma2, ma3);
32414 assertEquals(ma2, ma4);
32415
32416
32417 try {
32418 calibrator.getInitialMa(new Matrix(1, 3));
32419 fail("IllegalArgumentException expected but not thrown");
32420 } catch (final IllegalArgumentException ignore) {
32421 }
32422 try {
32423 calibrator.getInitialMa(new Matrix(3, 1));
32424 fail("IllegalArgumentException expected but not thrown");
32425 } catch (final IllegalArgumentException ignore) {
32426 }
32427 try {
32428 calibrator.setInitialMa(new Matrix(1, 3));
32429 fail("IllegalArgumentException expected but not thrown");
32430 } catch (final IllegalArgumentException ignore) {
32431 }
32432 try {
32433 calibrator.setInitialMa(new Matrix(3, 1));
32434 fail("IllegalArgumentException expected but not thrown");
32435 } catch (final IllegalArgumentException ignore) {
32436 }
32437 }
32438
32439 @Test
32440 public void testGetSetMeasurements() throws LockedException {
32441 final KnownGravityNormAccelerometerCalibrator calibrator =
32442 new KnownGravityNormAccelerometerCalibrator();
32443
32444
32445 assertNull(calibrator.getMeasurements());
32446
32447
32448 final Collection<StandardDeviationBodyKinematics> measurements =
32449 Collections.emptyList();
32450 calibrator.setMeasurements(measurements);
32451
32452
32453 assertSame(calibrator.getMeasurements(), measurements);
32454 }
32455
32456 @Test
32457 public void testIsSetCommonAxisUsed() throws LockedException {
32458 final KnownGravityNormAccelerometerCalibrator calibrator =
32459 new KnownGravityNormAccelerometerCalibrator();
32460
32461
32462 assertFalse(calibrator.isCommonAxisUsed());
32463
32464
32465 calibrator.setCommonAxisUsed(true);
32466
32467
32468 assertTrue(calibrator.isCommonAxisUsed());
32469 }
32470
32471 @Test
32472 public void testGetSetListener() throws LockedException {
32473 final KnownGravityNormAccelerometerCalibrator calibrator =
32474 new KnownGravityNormAccelerometerCalibrator();
32475
32476
32477 assertNull(calibrator.getListener());
32478
32479
32480 calibrator.setListener(this);
32481
32482
32483 assertSame(calibrator.getListener(), this);
32484 }
32485
32486 @Test
32487 public void testGetSetGroundTruthGravityNorm1() throws LockedException {
32488 final KnownGravityNormAccelerometerCalibrator calibrator =
32489 new KnownGravityNormAccelerometerCalibrator();
32490
32491
32492 assertNull(calibrator.getGroundTruthGravityNorm());
32493
32494
32495 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32496 final double latitude = Math.toRadians(
32497 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32498 final double longitude = Math.toRadians(
32499 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32500 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32501 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32502 final NEDVelocity nedVelocity = new NEDVelocity();
32503 final ECEFPosition ecefPosition = new ECEFPosition();
32504 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32505 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32506 ecefPosition, ecefVelocity);
32507 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32508 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32509 final double gravityNorm = gravity.getNorm();
32510
32511 calibrator.setGroundTruthGravityNorm(gravityNorm);
32512
32513
32514 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(),
32515 0.0);
32516 assertEquals(gravity.getNormAsAcceleration(),
32517 calibrator.getGroundTruthGravityNormAsAcceleration());
32518 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
32519 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
32520 assertEquals(gravity.getNormAsAcceleration(), g);
32521 }
32522
32523 @Test
32524 public void testGetSetGroundTruthGravityNorm2() throws LockedException {
32525 final KnownGravityNormAccelerometerCalibrator calibrator =
32526 new KnownGravityNormAccelerometerCalibrator();
32527
32528
32529 assertNull(calibrator.getGroundTruthGravityNorm());
32530
32531
32532 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32533 final double latitude = Math.toRadians(
32534 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32535 final double longitude = Math.toRadians(
32536 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32537 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32538 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32539 final NEDVelocity nedVelocity = new NEDVelocity();
32540 final ECEFPosition ecefPosition = new ECEFPosition();
32541 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32542 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32543 ecefPosition, ecefVelocity);
32544 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32545 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32546 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
32547
32548 calibrator.setGroundTruthGravityNorm(gravityNorm);
32549
32550
32551 assertEquals(gravity.getNorm(),
32552 calibrator.getGroundTruthGravityNorm(), 0.0);
32553 assertEquals(gravityNorm,
32554 calibrator.getGroundTruthGravityNormAsAcceleration());
32555 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
32556 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
32557 assertEquals(gravityNorm, g);
32558 }
32559
32560 @Test
32561 public void testIsReady() throws LockedException {
32562 final KnownGravityNormAccelerometerCalibrator calibrator =
32563 new KnownGravityNormAccelerometerCalibrator();
32564
32565
32566 assertFalse(calibrator.isReady());
32567
32568
32569
32570 final Collection<StandardDeviationBodyKinematics> measurements1 =
32571 Collections.emptyList();
32572 calibrator.setMeasurements(measurements1);
32573
32574
32575 assertFalse(calibrator.isReady());
32576
32577
32578
32579 calibrator.setCommonAxisUsed(false);
32580
32581 final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
32582 for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
32583 measurements2.add(new StandardDeviationBodyKinematics());
32584 }
32585 calibrator.setMeasurements(measurements2);
32586
32587
32588 assertFalse(calibrator.isReady());
32589
32590
32591
32592 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32593 final double latitude = Math.toRadians(
32594 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32595 final double longitude = Math.toRadians(
32596 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32597 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32598 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32599 final NEDVelocity nedVelocity = new NEDVelocity();
32600 final ECEFPosition ecefPosition = new ECEFPosition();
32601 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32602 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32603 ecefPosition, ecefVelocity);
32604 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32605 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32606
32607 calibrator.setGroundTruthGravityNorm(gravity.getNorm());
32608
32609 assertTrue(calibrator.isReady());
32610
32611
32612 measurements2.clear();
32613 for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
32614 measurements2.add(new StandardDeviationBodyKinematics());
32615 }
32616 calibrator.setMeasurements(measurements2);
32617
32618
32619 assertFalse(calibrator.isReady());
32620
32621 calibrator.setCommonAxisUsed(true);
32622
32623 assertTrue(calibrator.isReady());
32624 }
32625
32626 @Test
32627 public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
32628 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32629 LockedException, NotReadyException {
32630
32631 int numValid = 0;
32632 for (int t = 0; t < TIMES; t++) {
32633 final Matrix ba = generateBa();
32634 final Matrix bg = generateBg();
32635 final Matrix ma = generateMaGeneral();
32636 final Matrix mg = generateMg();
32637 final Matrix gg = generateGg();
32638
32639
32640 final double accelNoiseRootPSD = 0.0;
32641 final double gyroNoiseRootPSD = 0.0;
32642 final double accelQuantLevel = 0.0;
32643 final double gyroQuantLevel = 0.0;
32644
32645 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32646 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32647
32648 final Random random = new Random();
32649 final UniformRandomizer randomizer = new UniformRandomizer(random);
32650 final double latitude = Math.toRadians(
32651 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32652 final double longitude = Math.toRadians(
32653 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32654 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32655 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32656 final NEDVelocity nedVelocity = new NEDVelocity();
32657 final ECEFPosition ecefPosition = new ECEFPosition();
32658 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32659 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32660 ecefPosition, ecefVelocity);
32661 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32662 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32663
32664 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32665 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32666 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32667
32668 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32669 for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
32670 final double roll = Math.toRadians(
32671 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32672 final double pitch = Math.toRadians(
32673 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32674 final double yaw = Math.toRadians(
32675 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32676 final CoordinateTransformation nedC = new CoordinateTransformation(
32677 roll, pitch, yaw, FrameType.BODY_FRAME,
32678 FrameType.LOCAL_NAVIGATION_FRAME);
32679
32680 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32681 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32682 .convertNEDtoECEFAndReturnNew(nedFrame);
32683
32684
32685
32686 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32687 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32688 ecefFrame);
32689
32690
32691
32692 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32693 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32694
32695 final StandardDeviationBodyKinematics measurement =
32696 new StandardDeviationBodyKinematics(measuredKinematics,
32697 specificForceStandardDeviation,
32698 angularRateStandardDeviation);
32699 measurements.add(measurement);
32700 }
32701
32702
32703
32704 final KnownGravityNormAccelerometerCalibrator calibrator =
32705 new KnownGravityNormAccelerometerCalibrator(
32706 gravity.getNorm(), measurements,
32707 false, ba, ma, this);
32708
32709
32710 reset();
32711 assertTrue(calibrator.isReady());
32712 assertFalse(calibrator.isRunning());
32713 assertEquals(mCalibrateStart, 0);
32714 assertEquals(mCalibrateEnd, 0);
32715
32716 try {
32717 calibrator.calibrate();
32718 } catch (final CalibrationException e) {
32719 continue;
32720 }
32721
32722
32723 assertTrue(calibrator.isReady());
32724 assertFalse(calibrator.isRunning());
32725 assertEquals(mCalibrateStart, 1);
32726 assertEquals(mCalibrateEnd, 1);
32727
32728 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32729 final Matrix estimatedMa = calibrator.getEstimatedMa();
32730
32731 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
32732 continue;
32733 }
32734 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
32735 continue;
32736 }
32737
32738 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
32739 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
32740
32741 numValid++;
32742
32743 break;
32744 }
32745
32746 assertTrue(numValid > 0);
32747 }
32748
32749 @Test
32750 public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
32751 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32752 LockedException, NotReadyException {
32753
32754 int numValid = 0;
32755 for (int t = 0; t < TIMES; t++) {
32756 final Matrix ba = generateBa();
32757 final Matrix bg = generateBg();
32758 final Matrix ma = generateMaGeneral();
32759 final Matrix mg = generateMg();
32760 final Matrix gg = generateGg();
32761 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
32762 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
32763 final double accelQuantLevel = 0.0;
32764 final double gyroQuantLevel = 0.0;
32765
32766 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32767 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32768
32769 final Random random = new Random();
32770 final UniformRandomizer randomizer = new UniformRandomizer(random);
32771 final double latitude = Math.toRadians(
32772 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32773 final double longitude = Math.toRadians(
32774 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32775 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32776 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32777 final NEDVelocity nedVelocity = new NEDVelocity();
32778 final ECEFPosition ecefPosition = new ECEFPosition();
32779 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32780 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32781 ecefPosition, ecefVelocity);
32782 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32783 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32784
32785 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32786 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32787 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32788
32789 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32790 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
32791 final double roll = Math.toRadians(
32792 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32793 final double pitch = Math.toRadians(
32794 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32795 final double yaw = Math.toRadians(
32796 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32797 final CoordinateTransformation nedC = new CoordinateTransformation(
32798 roll, pitch, yaw, FrameType.BODY_FRAME,
32799 FrameType.LOCAL_NAVIGATION_FRAME);
32800
32801 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32802 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32803 .convertNEDtoECEFAndReturnNew(nedFrame);
32804
32805
32806
32807 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32808 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32809 ecefFrame);
32810
32811
32812
32813 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32814 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32815
32816 final StandardDeviationBodyKinematics measurement =
32817 new StandardDeviationBodyKinematics(measuredKinematics,
32818 specificForceStandardDeviation,
32819 angularRateStandardDeviation);
32820 measurements.add(measurement);
32821 }
32822
32823
32824
32825 final KnownGravityNormAccelerometerCalibrator calibrator =
32826 new KnownGravityNormAccelerometerCalibrator(
32827 gravity.getNorm(),
32828 measurements, false, this);
32829
32830
32831 reset();
32832 assertTrue(calibrator.isReady());
32833 assertFalse(calibrator.isRunning());
32834 assertEquals(mCalibrateStart, 0);
32835 assertEquals(mCalibrateEnd, 0);
32836
32837 try {
32838 calibrator.calibrate();
32839 } catch (final CalibrationException e) {
32840 continue;
32841 }
32842
32843
32844 assertTrue(calibrator.isReady());
32845 assertFalse(calibrator.isRunning());
32846 assertEquals(mCalibrateStart, 1);
32847 assertEquals(mCalibrateEnd, 1);
32848
32849 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32850 final Matrix estimatedMa = calibrator.getEstimatedMa();
32851
32852 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32853 continue;
32854 }
32855 if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
32856 continue;
32857 }
32858
32859 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32860 assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
32861
32862 numValid++;
32863
32864 break;
32865 }
32866
32867 assertTrue(numValid > 0);
32868 }
32869
32870 @Test
32871 public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
32872 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32873 LockedException, NotReadyException {
32874
32875 int numValid = 0;
32876 for (int t = 0; t < TIMES; t++) {
32877 final Matrix ba = generateBa();
32878 final Matrix bg = generateBg();
32879 final Matrix ma = generateMaCommonAxis();
32880 final Matrix mg = generateMg();
32881 final Matrix gg = generateGg();
32882
32883
32884 final double accelNoiseRootPSD = 0.0;
32885 final double gyroNoiseRootPSD = 0.0;
32886 final double accelQuantLevel = 0.0;
32887 final double gyroQuantLevel = 0.0;
32888
32889 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32890 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32891
32892 final Random random = new Random();
32893 final UniformRandomizer randomizer = new UniformRandomizer(random);
32894 final double latitude = Math.toRadians(
32895 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32896 final double longitude = Math.toRadians(
32897 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32898 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32899 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32900 final NEDVelocity nedVelocity = new NEDVelocity();
32901 final ECEFPosition ecefPosition = new ECEFPosition();
32902 final ECEFVelocity ecefVelocity = new ECEFVelocity();
32903 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32904 ecefPosition, ecefVelocity);
32905 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32906 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32907
32908 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32909 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32910 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32911
32912 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32913 for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
32914 final double roll = Math.toRadians(
32915 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32916 final double pitch = Math.toRadians(
32917 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32918 final double yaw = Math.toRadians(
32919 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32920 final CoordinateTransformation nedC = new CoordinateTransformation(
32921 roll, pitch, yaw, FrameType.BODY_FRAME,
32922 FrameType.LOCAL_NAVIGATION_FRAME);
32923
32924 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32925 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32926 .convertNEDtoECEFAndReturnNew(nedFrame);
32927
32928
32929
32930 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32931 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32932 ecefFrame);
32933
32934
32935
32936 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32937 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32938
32939 final StandardDeviationBodyKinematics measurement =
32940 new StandardDeviationBodyKinematics(measuredKinematics,
32941 specificForceStandardDeviation,
32942 angularRateStandardDeviation);
32943 measurements.add(measurement);
32944 }
32945
32946
32947
32948 final KnownGravityNormAccelerometerCalibrator calibrator =
32949 new KnownGravityNormAccelerometerCalibrator(
32950 gravity.getNorm(), measurements,
32951 true, ba, ma, this);
32952
32953
32954 reset();
32955 assertTrue(calibrator.isReady());
32956 assertFalse(calibrator.isRunning());
32957 assertEquals(mCalibrateStart, 0);
32958 assertEquals(mCalibrateEnd, 0);
32959
32960 try {
32961 calibrator.calibrate();
32962 } catch (final CalibrationException e) {
32963 continue;
32964 }
32965
32966
32967 assertTrue(calibrator.isReady());
32968 assertFalse(calibrator.isRunning());
32969 assertEquals(mCalibrateStart, 1);
32970 assertEquals(mCalibrateEnd, 1);
32971
32972 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32973 final Matrix estimatedMa = calibrator.getEstimatedMa();
32974
32975 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
32976 continue;
32977 }
32978 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
32979 continue;
32980 }
32981
32982 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
32983 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
32984
32985 numValid++;
32986
32987 break;
32988 }
32989
32990 assertTrue(numValid > 0);
32991 }
32992
32993 @Test
32994 public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
32995 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32996 LockedException, NotReadyException {
32997
32998 int numValid = 0;
32999 for (int t = 0; t < TIMES; t++) {
33000 final Matrix ba = generateBa();
33001 final Matrix bg = generateBg();
33002 final Matrix ma = generateMaCommonAxis();
33003 final Matrix mg = generateMg();
33004 final Matrix gg = generateGg();
33005 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
33006 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
33007 final double accelQuantLevel = 0.0;
33008 final double gyroQuantLevel = 0.0;
33009
33010 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
33011 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
33012
33013 final Random random = new Random();
33014 final UniformRandomizer randomizer = new UniformRandomizer(random);
33015 final double latitude = Math.toRadians(
33016 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
33017 final double longitude = Math.toRadians(
33018 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
33019 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
33020 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
33021 final NEDVelocity nedVelocity = new NEDVelocity();
33022 final ECEFPosition ecefPosition = new ECEFPosition();
33023 final ECEFVelocity ecefVelocity = new ECEFVelocity();
33024 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
33025 ecefPosition, ecefVelocity);
33026 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
33027 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
33028
33029 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
33030 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
33031 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
33032
33033 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
33034 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
33035 final double roll = Math.toRadians(
33036 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33037 final double pitch = Math.toRadians(
33038 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33039 final double yaw = Math.toRadians(
33040 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33041 final CoordinateTransformation nedC = new CoordinateTransformation(
33042 roll, pitch, yaw, FrameType.BODY_FRAME,
33043 FrameType.LOCAL_NAVIGATION_FRAME);
33044
33045 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
33046 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
33047 .convertNEDtoECEFAndReturnNew(nedFrame);
33048
33049
33050
33051 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
33052 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
33053 ecefFrame);
33054
33055
33056
33057 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
33058 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
33059
33060 final StandardDeviationBodyKinematics measurement =
33061 new StandardDeviationBodyKinematics(measuredKinematics,
33062 specificForceStandardDeviation,
33063 angularRateStandardDeviation);
33064 measurements.add(measurement);
33065 }
33066
33067
33068
33069 final KnownGravityNormAccelerometerCalibrator calibrator =
33070 new KnownGravityNormAccelerometerCalibrator(
33071 gravity.getNorm(), measurements,
33072 true, this);
33073
33074
33075 reset();
33076 assertTrue(calibrator.isReady());
33077 assertFalse(calibrator.isRunning());
33078 assertEquals(mCalibrateStart, 0);
33079 assertEquals(mCalibrateEnd, 0);
33080
33081 try {
33082 calibrator.calibrate();
33083 } catch (final CalibrationException e) {
33084 continue;
33085 }
33086
33087
33088 assertTrue(calibrator.isReady());
33089 assertFalse(calibrator.isRunning());
33090 assertEquals(mCalibrateStart, 1);
33091 assertEquals(mCalibrateEnd, 1);
33092
33093 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
33094 final Matrix estimatedMa = calibrator.getEstimatedMa();
33095
33096 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
33097 continue;
33098 }
33099 if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
33100 continue;
33101 }
33102
33103 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
33104 assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
33105
33106 numValid++;
33107
33108 break;
33109 }
33110
33111 assertTrue(numValid > 0);
33112 }
33113
33114 @Override
33115 public void onCalibrateStart(final KnownGravityNormAccelerometerCalibrator calibrator) {
33116 checkLocked(calibrator);
33117 mCalibrateStart++;
33118 }
33119
33120 @Override
33121 public void onCalibrateEnd(final KnownGravityNormAccelerometerCalibrator calibrator) {
33122 checkLocked(calibrator);
33123 mCalibrateEnd++;
33124 }
33125
33126 private void reset() {
33127 mCalibrateStart = 0;
33128 mCalibrateEnd = 0;
33129 }
33130
33131 private void checkLocked(final KnownGravityNormAccelerometerCalibrator calibrator) {
33132 assertTrue(calibrator.isRunning());
33133 try {
33134 calibrator.setInitialBiasX(0.0);
33135 fail("LockedException expected but not thrown");
33136 } catch (final LockedException ignore) {
33137 }
33138 try {
33139 calibrator.setInitialBiasY(0.0);
33140 fail("LockedException expected but not thrown");
33141 } catch (final LockedException ignore) {
33142 }
33143 try {
33144 calibrator.setInitialBiasZ(0.0);
33145 fail("LockedException expected but not thrown");
33146 } catch (final LockedException ignore) {
33147 }
33148 try {
33149 calibrator.setInitialBiasX(null);
33150 fail("LockedException expected but not thrown");
33151 } catch (final LockedException ignore) {
33152 }
33153 try {
33154 calibrator.setInitialBiasY(null);
33155 fail("LockedException expected but not thrown");
33156 } catch (final LockedException ignore) {
33157 }
33158 try {
33159 calibrator.setInitialBiasZ(null);
33160 fail("LockedException expected but not thrown");
33161 } catch (final LockedException ignore) {
33162 }
33163 try {
33164 calibrator.setInitialBias(0.0, 0.0, 0.0);
33165 fail("LockedException expected but not thrown");
33166 } catch (final LockedException ignore) {
33167 }
33168 try {
33169 calibrator.setInitialBias(null, null, null);
33170 fail("LockedException expected but not thrown");
33171 } catch (final LockedException ignore) {
33172 }
33173 try {
33174 calibrator.setInitialSx(0.0);
33175 fail("LockedException expected but not thrown");
33176 } catch (final LockedException ignore) {
33177 }
33178 try {
33179 calibrator.setInitialSy(0.0);
33180 fail("LockedException expected but not thrown");
33181 } catch (final LockedException ignore) {
33182 }
33183 try {
33184 calibrator.setInitialSz(0.0);
33185 fail("LockedException expected but not thrown");
33186 } catch (final LockedException ignore) {
33187 }
33188 try {
33189 calibrator.setInitialMxy(0.0);
33190 fail("LockedException expected but not thrown");
33191 } catch (final LockedException ignore) {
33192 }
33193 try {
33194 calibrator.setInitialMxz(0.0);
33195 fail("LockedException expected but not thrown");
33196 } catch (final LockedException ignore) {
33197 }
33198 try {
33199 calibrator.setInitialMyx(0.0);
33200 fail("LockedException expected but not thrown");
33201 } catch (final LockedException ignore) {
33202 }
33203 try {
33204 calibrator.setInitialMyz(0.0);
33205 fail("LockedException expected but not thrown");
33206 } catch (final LockedException ignore) {
33207 }
33208 try {
33209 calibrator.setInitialMzx(0.0);
33210 fail("LockedException expected but not thrown");
33211 } catch (final LockedException ignore) {
33212 }
33213 try {
33214 calibrator.setInitialMzy(0.0);
33215 fail("LockedException expected but not thrown");
33216 } catch (final LockedException ignore) {
33217 }
33218 try {
33219 calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
33220 fail("LockedException expected but not thrown");
33221 } catch (final LockedException ignore) {
33222 }
33223 try {
33224 calibrator.setInitialCrossCouplingErrors(
33225 0.0, 0.0, 0.0,
33226 0.0, 0.0, 0.0);
33227 fail("LockedException expected but not thrown");
33228 } catch (final LockedException ignore) {
33229 }
33230 try {
33231 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
33232 0.0, 0.0, 0.0,
33233 0.0, 0.0, 0.0,
33234 0.0, 0.0, 0.0);
33235 fail("LockedException expected but not thrown");
33236 } catch (final LockedException ignore) {
33237 }
33238 try {
33239 calibrator.setInitialBias((double[]) null);
33240 fail("LockedException expected but not thrown");
33241 } catch (final LockedException ignore) {
33242 }
33243 try {
33244 calibrator.setInitialBias((Matrix) null);
33245 fail("LockedException expected but not thrown");
33246 } catch (final LockedException ignore) {
33247 }
33248 try {
33249 calibrator.setInitialMa(null);
33250 fail("LockedException expected but not thrown");
33251 } catch (final LockedException ignore) {
33252 }
33253 try {
33254 calibrator.setGroundTruthGravityNorm(0.0);
33255 fail("LockedException expected but not thrown");
33256 } catch (final LockedException ignore) {
33257 }
33258 try {
33259 calibrator.setGroundTruthGravityNorm((Acceleration) null);
33260 fail("LockedException expected but not thrown");
33261 } catch (final LockedException ignore) {
33262 }
33263 try {
33264 calibrator.setMeasurements(null);
33265 fail("LockedException expected but not thrown");
33266 } catch (final LockedException ignore) {
33267 }
33268 try {
33269 calibrator.setCommonAxisUsed(true);
33270 fail("LockedException expected but not thrown");
33271 } catch (final LockedException ignore) {
33272 }
33273 try {
33274 calibrator.setListener(this);
33275 fail("LockedException expected but not thrown");
33276 } catch (final LockedException ignore) {
33277 }
33278 try {
33279 calibrator.calibrate();
33280 fail("LockedException expected but not thrown");
33281 } catch (final LockedException ignore) {
33282 } catch (final Exception e) {
33283 fail("LockedException expected but not thrown");
33284 }
33285 }
33286
33287 private Matrix generateBa() {
33288 return Matrix.newFromArray(new double[]{
33289 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
33290 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
33291 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
33292 }
33293
33294 private Matrix generateBg() {
33295 return Matrix.newFromArray(new double[]{
33296 -9 * DEG_TO_RAD / 3600.0,
33297 13 * DEG_TO_RAD / 3600.0,
33298 -8 * DEG_TO_RAD / 3600.0});
33299 }
33300
33301 private Matrix generateMaGeneral() throws WrongSizeException {
33302 final Matrix result = new Matrix(3, 3);
33303 result.fromArray(new double[]{
33304 500e-6, -300e-6, 200e-6,
33305 -150e-6, -600e-6, 250e-6,
33306 -250e-6, 100e-6, 450e-6
33307 }, false);
33308
33309 return result;
33310 }
33311
33312 private Matrix generateMaCommonAxis() throws WrongSizeException {
33313 final Matrix result = new Matrix(3, 3);
33314 result.fromArray(new double[]{
33315 500e-6, -300e-6, 200e-6,
33316 0.0, -600e-6, 250e-6,
33317 0.0, 0.0, 450e-6
33318 }, false);
33319
33320 return result;
33321 }
33322
33323 private Matrix generateMg() throws WrongSizeException {
33324 final Matrix result = new Matrix(3, 3);
33325 result.fromArray(new double[]{
33326 400e-6, -300e-6, 250e-6,
33327 0.0, -300e-6, -150e-6,
33328 0.0, 0.0, -350e-6
33329 }, false);
33330
33331 return result;
33332 }
33333
33334 private Matrix generateGg() throws WrongSizeException {
33335 final Matrix result = new Matrix(3, 3);
33336 final double tmp = DEG_TO_RAD / (3600 * 9.80665);
33337 result.fromArray(new double[]{
33338 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
33339 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
33340 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
33341 }, false);
33342
33343 return result;
33344 }
33345
33346 private double getAccelNoiseRootPSD() {
33347 return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
33348 }
33349
33350 private double getGyroNoiseRootPSD() {
33351 return 0.01 * DEG_TO_RAD / 60.0;
33352 }
33353 }